808 lines
25 KiB
C++
Executable File
808 lines
25 KiB
C++
Executable File
/*
|
|
* Copyright 2013 The Android Open Source Project
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#ifndef MATH_TMATHELPERS_H_
|
|
#define MATH_TMATHELPERS_H_
|
|
|
|
#include <math/compiler.h>
|
|
#include <math/quat.h>
|
|
#include <math/TVecHelpers.h>
|
|
|
|
#include <algorithm> // for std::swap
|
|
#include <cmath> // for std:: namespace
|
|
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <sys/types.h>
|
|
|
|
namespace filament {
|
|
namespace math {
|
|
namespace details {
|
|
// -------------------------------------------------------------------------------------
|
|
|
|
/*
|
|
* No user serviceable parts here.
|
|
*
|
|
* Don't use this file directly, instead include math/mat*.h
|
|
*/
|
|
|
|
|
|
/*
|
|
* Matrix utilities
|
|
*/
|
|
|
|
namespace matrix {
|
|
|
|
/*
|
|
* Matrix inversion
|
|
*/
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE gaussJordanInverse(MATRIX src) {
|
|
typedef typename MATRIX::value_type T;
|
|
constexpr unsigned int N = MATRIX::NUM_ROWS;
|
|
MATRIX inverted;
|
|
|
|
for (size_t i = 0; i < N; ++i) {
|
|
// look for largest element in i'th column
|
|
size_t swap = i;
|
|
T t = src[i][i] < 0 ? -src[i][i] : src[i][i];
|
|
for (size_t j = i + 1; j < N; ++j) {
|
|
const T t2 = src[j][i] < 0 ? -src[j][i] : src[j][i];
|
|
if (t2 > t) {
|
|
swap = j;
|
|
t = t2;
|
|
}
|
|
}
|
|
|
|
if (swap != i) {
|
|
// swap columns.
|
|
std::swap(src[i], src[swap]);
|
|
std::swap(inverted[i], inverted[swap]);
|
|
}
|
|
|
|
const T denom(src[i][i]);
|
|
for (size_t k = 0; k < N; ++k) {
|
|
src[i][k] /= denom;
|
|
inverted[i][k] /= denom;
|
|
}
|
|
|
|
// Factor out the lower triangle
|
|
for (size_t j = 0; j < N; ++j) {
|
|
if (j != i) {
|
|
const T t = src[j][i];
|
|
for (size_t k = 0; k < N; ++k) {
|
|
src[j][k] -= src[i][k] * t;
|
|
inverted[j][k] -= inverted[i][k] * t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return inverted;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
// 2x2 matrix inverse is easy.
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE fastInverse2(const MATRIX& x) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
// Assuming the input matrix is:
|
|
// | a b |
|
|
// | c d |
|
|
//
|
|
// The analytic inverse is
|
|
// | d -b |
|
|
// | -c a | / (a d - b c)
|
|
//
|
|
// Importantly, our matrices are column-major!
|
|
|
|
MATRIX inverted{};
|
|
|
|
const T a = x[0][0];
|
|
const T c = x[0][1];
|
|
const T b = x[1][0];
|
|
const T d = x[1][1];
|
|
|
|
const T det((a * d) - (b * c));
|
|
inverted[0][0] = d / det;
|
|
inverted[0][1] = -c / det;
|
|
inverted[1][0] = -b / det;
|
|
inverted[1][1] = a / det;
|
|
return inverted;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
// From the Wikipedia article on matrix inversion's section on fast 3x3
|
|
// matrix inversion:
|
|
// http://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3.C3.973_matrices
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE fastInverse3(const MATRIX& x) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
// Assuming the input matrix is:
|
|
// | a b c |
|
|
// | d e f |
|
|
// | g h i |
|
|
//
|
|
// The analytic inverse is
|
|
// | A B C |^T
|
|
// | D E F |
|
|
// | G H I | / determinant
|
|
//
|
|
// Which is
|
|
// | A D G |
|
|
// | B E H |
|
|
// | C F I | / determinant
|
|
//
|
|
// Where:
|
|
// A = (ei - fh), B = (fg - di), C = (dh - eg)
|
|
// D = (ch - bi), E = (ai - cg), F = (bg - ah)
|
|
// G = (bf - ce), H = (cd - af), I = (ae - bd)
|
|
//
|
|
// and the determinant is a*A + b*B + c*C (The rule of Sarrus)
|
|
//
|
|
// Importantly, our matrices are column-major!
|
|
|
|
MATRIX inverted{};
|
|
|
|
const T a = x[0][0];
|
|
const T b = x[1][0];
|
|
const T c = x[2][0];
|
|
const T d = x[0][1];
|
|
const T e = x[1][1];
|
|
const T f = x[2][1];
|
|
const T g = x[0][2];
|
|
const T h = x[1][2];
|
|
const T i = x[2][2];
|
|
|
|
// Do the full analytic inverse
|
|
const T A = e * i - f * h;
|
|
const T B = f * g - d * i;
|
|
const T C = d * h - e * g;
|
|
inverted[0][0] = A; // A
|
|
inverted[0][1] = B; // B
|
|
inverted[0][2] = C; // C
|
|
inverted[1][0] = c * h - b * i; // D
|
|
inverted[1][1] = a * i - c * g; // E
|
|
inverted[1][2] = b * g - a * h; // F
|
|
inverted[2][0] = b * f - c * e; // G
|
|
inverted[2][1] = c * d - a * f; // H
|
|
inverted[2][2] = a * e - b * d; // I
|
|
|
|
const T det(a * A + b * B + c * C);
|
|
for (size_t col = 0; col < 3; ++col) {
|
|
for (size_t row = 0; row < 3; ++row) {
|
|
inverted[col][row] /= det;
|
|
}
|
|
}
|
|
|
|
return inverted;
|
|
}
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Determinant and cofactor
|
|
|
|
// this is just a dummy matrix helper
|
|
template<typename T, size_t ORDER>
|
|
class Matrix {
|
|
T m[ORDER][ORDER];
|
|
public:
|
|
constexpr auto operator[](size_t i) const noexcept { return m[i]; }
|
|
|
|
constexpr auto& operator[](size_t i) noexcept { return m[i]; }
|
|
|
|
static constexpr Matrix<T, ORDER - 1> submatrix(Matrix in, size_t row, size_t col) noexcept {
|
|
size_t colCount = 0, rowCount = 0;
|
|
Matrix<T, ORDER - 1> dest{};
|
|
for (size_t i = 0; i < ORDER; i++) {
|
|
if (i != row) {
|
|
colCount = 0;
|
|
for (size_t j = 0; j < ORDER; j++) {
|
|
if (j != col) {
|
|
dest[rowCount][colCount] = in[i][j];
|
|
colCount++;
|
|
}
|
|
}
|
|
rowCount++;
|
|
}
|
|
}
|
|
return dest;
|
|
}
|
|
};
|
|
|
|
template<typename T, size_t O>
|
|
struct Determinant {
|
|
static constexpr T determinant(Matrix<T, O> in) {
|
|
T det = {};
|
|
for (size_t i = 0; i < O; i++) {
|
|
T m = Determinant<T, O - 1>::determinant(Matrix<T, O>::submatrix(in, 0, i));
|
|
T factor = (i % 2 == 1) ? T(-1) : T(1);
|
|
det += factor * in[0][i] * m;
|
|
}
|
|
return det;
|
|
}
|
|
};
|
|
|
|
template<typename T>
|
|
struct Determinant<T, 3> {
|
|
static constexpr T determinant(Matrix<T, 3> in) {
|
|
return
|
|
in[0][0] * in[1][1] * in[2][2] +
|
|
in[1][0] * in[2][1] * in[0][2] +
|
|
in[2][0] * in[0][1] * in[1][2] -
|
|
in[2][0] * in[1][1] * in[0][2] -
|
|
in[1][0] * in[0][1] * in[2][2] -
|
|
in[0][0] * in[2][1] * in[1][2];
|
|
}
|
|
};
|
|
|
|
template<typename T>
|
|
struct Determinant<T, 2> {
|
|
static constexpr T determinant(Matrix<T, 2> in) {
|
|
return in[0][0] * in[1][1] - in[0][1] * in[1][0];
|
|
}
|
|
};
|
|
|
|
template<typename T>
|
|
struct Determinant<T, 1> {
|
|
static constexpr T determinant(Matrix<T, 1> in) { return in[0][0]; }
|
|
};
|
|
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE cofactor(const MATRIX& m) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
MATRIX out;
|
|
constexpr size_t order = MATRIX::NUM_COLS;
|
|
|
|
Matrix<T, order> in{};
|
|
for (size_t i = 0; i < order; i++) {
|
|
for (size_t j = 0; j < order; j++) {
|
|
in[i][j] = m[i][j];
|
|
}
|
|
}
|
|
|
|
for (size_t i = 0; i < order; i++) {
|
|
for (size_t j = 0; j < order; j++) {
|
|
T factor = ((i + j) % 2 == 1) ? T(-1) : T(1);
|
|
out[i][j] = Determinant<T, order - 1>::determinant(
|
|
Matrix<T, order>::submatrix(in, i, j)) * factor;
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE fastCofactor2(const MATRIX& m) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
// Assuming the input matrix is:
|
|
// | a b |
|
|
// | c d |
|
|
//
|
|
// The cofactor are
|
|
// | d -c |
|
|
// | -b a |
|
|
//
|
|
// Importantly, our matrices are column-major!
|
|
|
|
MATRIX cof{};
|
|
|
|
const T a = m[0][0];
|
|
const T c = m[0][1];
|
|
const T b = m[1][0];
|
|
const T d = m[1][1];
|
|
|
|
cof[0][0] = d;
|
|
cof[0][1] = -b;
|
|
cof[1][0] = -c;
|
|
cof[1][1] = a;
|
|
return cof;
|
|
}
|
|
|
|
template<typename MATRIX>
|
|
constexpr MATRIX MATH_PURE fastCofactor3(const MATRIX& m) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
// Assuming the input matrix is:
|
|
// | a b c |
|
|
// | d e f |
|
|
// | g h i |
|
|
//
|
|
// The cofactor are
|
|
// | A B C |
|
|
// | D E F |
|
|
// | G H I |
|
|
|
|
// Where:
|
|
// A = (ei - fh), B = (fg - di), C = (dh - eg)
|
|
// D = (ch - bi), E = (ai - cg), F = (bg - ah)
|
|
// G = (bf - ce), H = (cd - af), I = (ae - bd)
|
|
|
|
// Importantly, our matrices are column-major!
|
|
|
|
MATRIX cof{};
|
|
|
|
const T a = m[0][0];
|
|
const T b = m[1][0];
|
|
const T c = m[2][0];
|
|
const T d = m[0][1];
|
|
const T e = m[1][1];
|
|
const T f = m[2][1];
|
|
const T g = m[0][2];
|
|
const T h = m[1][2];
|
|
const T i = m[2][2];
|
|
|
|
cof[0][0] = e * i - f * h; // A
|
|
cof[0][1] = c * h - b * i; // D
|
|
cof[0][2] = b * f - c * e; // G
|
|
cof[1][0] = f * g - d * i; // B
|
|
cof[1][1] = a * i - c * g; // E
|
|
cof[1][2] = c * d - a * f; // H
|
|
cof[2][0] = d * h - e * g; // C
|
|
cof[2][1] = b * g - a * h; // F
|
|
cof[2][2] = a * e - b * d; // I
|
|
|
|
return cof;
|
|
}
|
|
|
|
|
|
/**
|
|
* Cofactor function which switches on the matrix size.
|
|
*/
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr MATRIX MATH_PURE cof(const MATRIX& matrix) {
|
|
return (MATRIX::NUM_ROWS == 2) ? fastCofactor2<MATRIX>(matrix) :
|
|
((MATRIX::NUM_ROWS == 3) ? fastCofactor3<MATRIX>(matrix) :
|
|
cofactor<MATRIX>(matrix));
|
|
}
|
|
|
|
/**
|
|
* Determinant of a matrix
|
|
*/
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr typename MATRIX::value_type MATH_PURE det(const MATRIX& matrix) {
|
|
typedef typename MATRIX::value_type T;
|
|
constexpr unsigned int N = MATRIX::NUM_ROWS;
|
|
Matrix<T, N> in{};
|
|
for (size_t i = 0; i < N; i++) {
|
|
for (size_t j = 0; j < N; j++) {
|
|
in[i][j] = matrix[i][j];
|
|
}
|
|
}
|
|
return Determinant<typename MATRIX::value_type, MATRIX::NUM_COLS>::determinant(in);
|
|
}
|
|
|
|
/**
|
|
* Inversion function which switches on the matrix size.
|
|
* @warning This function assumes the matrix is invertible. The result is
|
|
* undefined if it is not. It is the responsibility of the caller to
|
|
* make sure the matrix is not singular.
|
|
*/
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr MATRIX MATH_PURE inverse(const MATRIX& matrix) {
|
|
return (MATRIX::NUM_ROWS == 2) ? fastInverse2<MATRIX>(matrix) :
|
|
((MATRIX::NUM_ROWS == 3) ? fastInverse3<MATRIX>(matrix) :
|
|
gaussJordanInverse<MATRIX>(matrix));
|
|
}
|
|
|
|
template<typename MATRIX_R, typename MATRIX_A, typename MATRIX_B,
|
|
typename = std::enable_if_t<
|
|
MATRIX_A::NUM_COLS == MATRIX_B::NUM_ROWS &&
|
|
MATRIX_R::NUM_COLS == MATRIX_B::NUM_COLS &&
|
|
MATRIX_R::NUM_ROWS == MATRIX_A::NUM_ROWS, int>>
|
|
constexpr MATRIX_R MATH_PURE multiply(MATRIX_A lhs, MATRIX_B rhs) {
|
|
// pre-requisite:
|
|
// lhs : D columns, R rows
|
|
// rhs : C columns, D rows
|
|
// res : C columns, R rows
|
|
MATRIX_R res{};
|
|
for (size_t col = 0; col < MATRIX_R::NUM_COLS; ++col) {
|
|
res[col] = lhs * rhs[col];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr MATRIX MATH_PURE transpose(MATRIX m) {
|
|
// for now we only handle square matrix transpose
|
|
MATRIX result{};
|
|
for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
|
|
for (size_t row = 0; row < MATRIX::NUM_ROWS; ++row) {
|
|
result[col][row] = m[row][col];
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr typename MATRIX::value_type MATH_PURE trace(MATRIX m) {
|
|
typename MATRIX::value_type result{};
|
|
for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
|
|
result += m[col][col];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
template<typename MATRIX,
|
|
typename = std::enable_if_t<MATRIX::NUM_ROWS == MATRIX::NUM_COLS, int>>
|
|
inline constexpr typename MATRIX::col_type MATH_PURE diag(MATRIX m) {
|
|
typename MATRIX::col_type result{};
|
|
for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
|
|
result[col] = m[col][col];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
// This is taken from the Imath MatrixAlgo code, and is identical to Eigen.
|
|
template<typename MATRIX>
|
|
TQuaternion<typename MATRIX::value_type> extractQuat(const MATRIX& mat) {
|
|
typedef typename MATRIX::value_type T;
|
|
|
|
TQuaternion<T> quat(TQuaternion<T>::NO_INIT);
|
|
|
|
// Compute the trace to see if it is positive or not.
|
|
const T trace = mat[0][0] + mat[1][1] + mat[2][2];
|
|
|
|
// check the sign of the trace
|
|
if (MATH_LIKELY(trace > 0)) {
|
|
// trace is positive
|
|
T s = std::sqrt(trace + 1);
|
|
quat.w = T(0.5) * s;
|
|
s = T(0.5) / s;
|
|
quat.x = (mat[1][2] - mat[2][1]) * s;
|
|
quat.y = (mat[2][0] - mat[0][2]) * s;
|
|
quat.z = (mat[0][1] - mat[1][0]) * s;
|
|
} else {
|
|
// trace is negative
|
|
|
|
// Find the index of the greatest diagonal
|
|
size_t i = 0;
|
|
if (mat[1][1] > mat[0][0]) { i = 1; }
|
|
if (mat[2][2] > mat[i][i]) { i = 2; }
|
|
|
|
// Get the next indices: (n+1)%3
|
|
static constexpr size_t next_ijk[3] = { 1, 2, 0 };
|
|
size_t j = next_ijk[i];
|
|
size_t k = next_ijk[j];
|
|
T s = std::sqrt((mat[i][i] - (mat[j][j] + mat[k][k])) + 1);
|
|
quat[i] = T(0.5) * s;
|
|
if (s != 0) {
|
|
s = T(0.5) / s;
|
|
}
|
|
quat.w = (mat[j][k] - mat[k][j]) * s;
|
|
quat[j] = (mat[i][j] + mat[j][i]) * s;
|
|
quat[k] = (mat[i][k] + mat[k][i]) * s;
|
|
}
|
|
return quat;
|
|
}
|
|
|
|
} // namespace matrix
|
|
|
|
// -------------------------------------------------------------------------------------
|
|
|
|
/*
|
|
* TMatProductOperators implements basic arithmetic and basic compound assignments
|
|
* operators on a vector of type BASE<T>.
|
|
*
|
|
* BASE only needs to implement operator[] and size().
|
|
* By simply inheriting from TMatProductOperators<BASE, T> BASE will automatically
|
|
* get all the functionality here.
|
|
*/
|
|
|
|
template<template<typename> class BASE, typename T,
|
|
template<typename> class VEC>
|
|
class TMatProductOperators {
|
|
public:
|
|
// matrix *= matrix
|
|
template<typename U>
|
|
constexpr BASE<T>& operator*=(const BASE<U>& rhs) {
|
|
BASE<T>& lhs(static_cast< BASE<T>& >(*this));
|
|
lhs = matrix::multiply<BASE<T>>(lhs, rhs);
|
|
return lhs;
|
|
}
|
|
|
|
// matrix *= scalar
|
|
template<typename U, typename = enable_if_arithmetic_t<U>>
|
|
constexpr BASE<T>& operator*=(U v) {
|
|
BASE<T>& lhs(static_cast< BASE<T>& >(*this));
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
lhs[col] *= v;
|
|
}
|
|
return lhs;
|
|
}
|
|
|
|
// matrix /= scalar
|
|
template<typename U, typename = enable_if_arithmetic_t<U>>
|
|
constexpr BASE<T>& operator/=(U v) {
|
|
BASE<T>& lhs(static_cast< BASE<T>& >(*this));
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
lhs[col] /= v;
|
|
}
|
|
return lhs;
|
|
}
|
|
|
|
private:
|
|
/*
|
|
* NOTE: the functions below ARE NOT member methods. They are friend functions
|
|
* with they definition inlined with their declaration. This makes these
|
|
* template functions available to the compiler when (and only when) this class
|
|
* is instantiated, at which point they're only templated on the 2nd parameter
|
|
* (the first one, BASE<T> being known).
|
|
*/
|
|
|
|
// matrix * matrix
|
|
template<typename U>
|
|
friend inline constexpr BASE<arithmetic_result_t<T, U>> MATH_PURE
|
|
operator*(BASE<T> lhs, BASE<U> rhs) {
|
|
return matrix::multiply<BASE<arithmetic_result_t<T, U>>>(lhs, rhs);
|
|
}
|
|
|
|
// matrix * vector
|
|
template<typename U>
|
|
friend inline constexpr typename BASE<arithmetic_result_t<T, U>>::col_type MATH_PURE
|
|
operator*(const BASE<T>& lhs, const VEC<U>& rhs) {
|
|
typename BASE<arithmetic_result_t<T, U>>::col_type result{};
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
result += lhs[col] * rhs[col];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
// row-vector * matrix
|
|
template<typename U>
|
|
friend inline constexpr typename BASE<arithmetic_result_t<T, U>>::row_type MATH_PURE
|
|
operator*(const VEC<U>& lhs, const BASE<T>& rhs) {
|
|
typename BASE<arithmetic_result_t<T, U>>::row_type result{};
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
result[col] = dot(lhs, rhs[col]);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
// matrix * scalar
|
|
template<typename U, typename = enable_if_arithmetic_t <U>>
|
|
friend inline constexpr BASE<arithmetic_result_t < T, U>> MATH_PURE
|
|
operator*(const BASE<T>& lhs, U rhs) {
|
|
BASE<arithmetic_result_t<T, U>> result{};
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
result[col] = lhs[col] * rhs;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
// scalar * matrix
|
|
template<typename U, typename = enable_if_arithmetic_t<U>>
|
|
friend inline constexpr BASE<arithmetic_result_t<T, U>> MATH_PURE
|
|
operator*(U rhs, const BASE<T>& lhs) {
|
|
return lhs * rhs;
|
|
}
|
|
|
|
// matrix / scalar
|
|
template<typename U, typename = enable_if_arithmetic_t<U>>
|
|
friend inline constexpr BASE<arithmetic_result_t<T, U>> MATH_PURE
|
|
operator/(const BASE<T>& lhs, U rhs) {
|
|
BASE<arithmetic_result_t<T, U>> result{};
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
result[col] = lhs[col] / rhs;
|
|
}
|
|
return result;
|
|
}
|
|
};
|
|
|
|
/*
|
|
* TMatSquareFunctions implements functions on a matrix of type BASE<T>.
|
|
*
|
|
* BASE only needs to implement:
|
|
* - operator[]
|
|
* - col_type
|
|
* - row_type
|
|
* - COL_SIZE
|
|
* - ROW_SIZE
|
|
*
|
|
* By simply inheriting from TMatSquareFunctions<BASE, T> BASE will automatically
|
|
* get all the functionality here.
|
|
*/
|
|
|
|
template<template<typename U> class BASE, typename T>
|
|
class TMatSquareFunctions {
|
|
private:
|
|
/*
|
|
* NOTE: the functions below ARE NOT member methods. They are friend functions
|
|
* with they definition inlined with their declaration. This makes these
|
|
* template functions available to the compiler when (and only when) this class
|
|
* is instantiated, at which point they're only templated on the 2nd parameter
|
|
* (the first one, BASE<T> being known).
|
|
*/
|
|
friend inline constexpr BASE<T> MATH_PURE inverse(const BASE<T>& matrix) {
|
|
return matrix::inverse(matrix);
|
|
}
|
|
|
|
friend inline constexpr BASE<T> MATH_PURE cof(const BASE<T>& matrix) {
|
|
return matrix::cof(matrix);
|
|
}
|
|
|
|
friend inline constexpr BASE<T> MATH_PURE transpose(BASE<T> m) {
|
|
return matrix::transpose(m);
|
|
}
|
|
|
|
friend inline constexpr T MATH_PURE trace(BASE<T> m) {
|
|
return matrix::trace(m);
|
|
}
|
|
|
|
friend inline constexpr T MATH_PURE det(const BASE<T>& m) {
|
|
return matrix::det(m);
|
|
}
|
|
|
|
// unclear why we have to use 'auto' here. 'typename BASE<T>::col_type' produces
|
|
// error: no type named 'col_type' in 'filament::math::details::TMat44<float>'
|
|
friend inline constexpr auto MATH_PURE diag(const BASE<T>& m) {
|
|
return matrix::diag(m);
|
|
}
|
|
};
|
|
|
|
template<template<typename U> class BASE, typename T>
|
|
class TMatHelpers {
|
|
public:
|
|
constexpr inline size_t getColumnSize() const { return BASE<T>::COL_SIZE; }
|
|
constexpr inline size_t getRowSize() const { return BASE<T>::ROW_SIZE; }
|
|
constexpr inline size_t getColumnCount() const { return BASE<T>::NUM_COLS; }
|
|
constexpr inline size_t getRowCount() const { return BASE<T>::NUM_ROWS; }
|
|
constexpr inline size_t size() const { return BASE<T>::ROW_SIZE; } // for TVec*<>
|
|
|
|
// array access
|
|
constexpr T const* asArray() const {
|
|
return &static_cast<BASE<T> const &>(*this)[0][0];
|
|
}
|
|
|
|
// element access
|
|
inline constexpr T const& operator()(size_t row, size_t col) const {
|
|
return static_cast<BASE<T> const &>(*this)[col][row];
|
|
}
|
|
|
|
inline T& operator()(size_t row, size_t col) {
|
|
return static_cast<BASE<T>&>(*this)[col][row];
|
|
}
|
|
|
|
private:
|
|
constexpr friend inline BASE<T> MATH_PURE abs(BASE<T> m) {
|
|
for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
|
|
m[col] = abs(m[col]);
|
|
}
|
|
return m;
|
|
}
|
|
};
|
|
|
|
// functions for 3x3 and 4x4 matrices
|
|
template<template<typename U> class BASE, typename T>
|
|
class TMatTransform {
|
|
public:
|
|
inline constexpr TMatTransform() {
|
|
static_assert(BASE<T>::NUM_ROWS == 3 || BASE<T>::NUM_ROWS == 4, "3x3 or 4x4 matrices only");
|
|
}
|
|
|
|
template<typename A, typename VEC, typename = enable_if_arithmetic_t<A>>
|
|
static BASE<T> rotation(A radian, VEC about) {
|
|
BASE<T> r;
|
|
T c = std::cos(radian);
|
|
T s = std::sin(radian);
|
|
if (about[0] == 1 && about[1] == 0 && about[2] == 0) {
|
|
r[1][1] = c; r[2][2] = c;
|
|
r[1][2] = s; r[2][1] = -s;
|
|
} else if (about[0] == 0 && about[1] == 1 && about[2] == 0) {
|
|
r[0][0] = c; r[2][2] = c;
|
|
r[2][0] = s; r[0][2] = -s;
|
|
} else if (about[0] == 0 && about[1] == 0 && about[2] == 1) {
|
|
r[0][0] = c; r[1][1] = c;
|
|
r[0][1] = s; r[1][0] = -s;
|
|
} else {
|
|
VEC nabout = normalize(about);
|
|
typename VEC::value_type x = nabout[0];
|
|
typename VEC::value_type y = nabout[1];
|
|
typename VEC::value_type z = nabout[2];
|
|
T nc = 1 - c;
|
|
T xy = x * y;
|
|
T yz = y * z;
|
|
T zx = z * x;
|
|
T xs = x * s;
|
|
T ys = y * s;
|
|
T zs = z * s;
|
|
r[0][0] = x*x*nc + c; r[1][0] = xy*nc - zs; r[2][0] = zx*nc + ys;
|
|
r[0][1] = xy*nc + zs; r[1][1] = y*y*nc + c; r[2][1] = yz*nc - xs;
|
|
r[0][2] = zx*nc - ys; r[1][2] = yz*nc + xs; r[2][2] = z*z*nc + c;
|
|
|
|
// Clamp results to -1, 1.
|
|
for (size_t col = 0; col < 3; ++col) {
|
|
for (size_t row = 0; row < 3; ++row) {
|
|
r[col][row] = std::min(std::max(r[col][row], T(-1)), T(1));
|
|
}
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
/**
|
|
* Create a matrix from euler angles using YPR around YXZ respectively
|
|
* @param yaw about Y axis
|
|
* @param pitch about X axis
|
|
* @param roll about Z axis
|
|
*/
|
|
template<typename Y, typename P, typename R, typename = enable_if_arithmetic_t<Y, P, R>>
|
|
static BASE<T> eulerYXZ(Y yaw, P pitch, R roll) {
|
|
return eulerZYX(roll, pitch, yaw);
|
|
}
|
|
|
|
/**
|
|
* Create a matrix from euler angles using YPR around ZYX respectively
|
|
* @param roll about X axis
|
|
* @param pitch about Y axis
|
|
* @param yaw about Z axis
|
|
*
|
|
* The euler angles are applied in ZYX order. i.e: a vector is first rotated
|
|
* about X (roll) then Y (pitch) and then Z (yaw).
|
|
*/
|
|
template<typename Y, typename P, typename R, typename = enable_if_arithmetic_t<Y, P, R>>
|
|
static BASE<T> eulerZYX(Y yaw, P pitch, R roll) {
|
|
BASE<T> r;
|
|
T cy = std::cos(yaw);
|
|
T sy = std::sin(yaw);
|
|
T cp = std::cos(pitch);
|
|
T sp = std::sin(pitch);
|
|
T cr = std::cos(roll);
|
|
T sr = std::sin(roll);
|
|
T cc = cr * cy;
|
|
T cs = cr * sy;
|
|
T sc = sr * cy;
|
|
T ss = sr * sy;
|
|
r[0][0] = cp * cy;
|
|
r[0][1] = cp * sy;
|
|
r[0][2] = -sp;
|
|
r[1][0] = sp * sc - cs;
|
|
r[1][1] = sp * ss + cc;
|
|
r[1][2] = cp * sr;
|
|
r[2][0] = sp * cc + ss;
|
|
r[2][1] = sp * cs - sc;
|
|
r[2][2] = cp * cr;
|
|
|
|
// Clamp results to -1, 1.
|
|
for (size_t col = 0; col < 3; ++col) {
|
|
for (size_t row = 0; row < 3; ++row) {
|
|
r[col][row] = std::min(std::max(r[col][row], T(-1)), T(1));
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
TQuaternion <T> toQuaternion() const {
|
|
return matrix::extractQuat(static_cast<const BASE<T>&>(*this));
|
|
}
|
|
};
|
|
|
|
// -------------------------------------------------------------------------------------
|
|
} // namespace details
|
|
} // namespace math
|
|
} // namespace filament
|
|
|
|
#endif // MATH_TMATHELPERS_H_
|