mirror/VulkanMemoryAllocator
1
0
Fork 0
mirror of https://github.com/GPUOpen-LibrariesAndSDKs/VulkanMemoryAllocator.git synced 2024-11-10 10:41:52 +00:00
Code Issues Packages Projects Releases Wiki Activity
5268dbbce5
VulkanMemoryAllocator/third_party/mathfu-1.1.0/include/mathfu/matrix.h
Adam Sawicki e6e498f63a First commit
2017-06-16 17:21:31 +02:00

1543 lines
58 KiB
C++
Raw Blame History

/*
* Copyright 2014 Google Inc. All rights reserved.
*
* 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 MATHFU_MATRIX_H_
#define MATHFU_MATRIX_H_
#include "mathfu/utilities.h"
#include "mathfu/vector.h"
#include <cmath>
#include <assert.h>
/// @file mathfu/matrix.h
/// @brief Matrix class and functions.
/// @addtogroup mathfu_matrix
///
/// MathFu provides a generic Matrix implementation which is specialized
/// for 4x4 matrices to take advantage of optimization opportunities using
/// SIMD instructions.
#ifdef _MSC_VER
#pragma warning(push)
// The following disables warnings for MATHFU_MAT_OPERATION.
// The buffer overrun warning must be disabled as MSVC doesn't treat
// "columns" as constant and therefore assumes that it's possible
// to overrun arrays indexed by "i".
// The conditional expression is constant warning is disabled since
// MSVC decides that "columns" *is* constant when unrolling the operation
// loop.
#pragma warning(disable : 4127) // conditional expression is constant
#pragma warning(disable : 4789) // buffer overrun
#if _MSC_VER >= 1900 // MSVC 2015
#pragma warning(disable : 4456) // allow shadowing in unrolled loops
#pragma warning(disable : 4723) // suppress "potential divide by 0" warning
#endif // _MSC_VER >= 1900
#endif // _MSC_VER
/// @cond MATHFU_INTERNAL
/// The stride of a vector (e.g Vector<T, 3>) when cast as an array of floats.
#define MATHFU_VECTOR_STRIDE_FLOATS(vector) (sizeof(vector) / sizeof(float))
/// @endcond
/// @cond MATHFU_INTERNAL
/// This will unroll loops for matrices with <= 4 columns
#define MATHFU_MAT_OPERATION(OP) MATHFU_UNROLLED_LOOP(i, columns, OP)
/// @endcond
/// @cond MATHFU_INTERNAL
/// This will perform a given OP on each matrix column and return the result
#define MATHFU_MAT_OPERATOR(OP) \
{ \
Matrix<T, rows, columns> result; \
MATHFU_MAT_OPERATION(result.data_[i] = (OP)); \
return result; \
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// This will perform a given OP on each matrix column
#define MATHFU_MAT_SELF_OPERATOR(OP) \
{ \
MATHFU_MAT_OPERATION(OP); \
return *this; \
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// This macro will take the dot product for a row from data1 and a column from
/// data2.
#define MATHFU_MATRIX_4X4_DOT(data1, data2, r) \
((data1)[r] * (data2)[0] + (data1)[(r) + 4] * (data2)[1] + \
(data1)[(r) + 8] * (data2)[2] + (data1)[(r) + 12] * (data2)[3])
/// @endcond
/// @cond MATHFU_INTERNAL
#define MATHFU_MATRIX_3X3_DOT(data1, data2, r, size) \
((data1)[r] * (data2)[0] + (data1)[(r) + (size)] * (data2)[1] + \
(data1)[(r) + 2 * (size)] * (data2)[2])
/// @endcond
namespace mathfu {
/// @cond MATHFU_INTERNAL
template <class T, int rows, int columns = rows>
class Matrix;
template <class T, int rows, int columns>
inline Matrix<T, rows, columns> IdentityHelper();
template <bool check_invertible, class T, int rows, int columns>
inline bool InverseHelper(const Matrix<T, rows, columns>& m,
Matrix<T, rows, columns>* const inverse);
template <class T, int rows, int columns>
inline void TimesHelper(const Matrix<T, rows, columns>& m1,
const Matrix<T, rows, columns>& m2,
Matrix<T, rows, columns>* out_m);
template <class T, int rows, int columns>
static inline Matrix<T, rows, columns> OuterProductHelper(
const Vector<T, rows>& v1, const Vector<T, columns>& v2);
template <class T>
inline Matrix<T, 4, 4> PerspectiveHelper(T fovy, T aspect, T znear, T zfar,
T handedness);
template <class T>
static inline Matrix<T, 4, 4> OrthoHelper(T left, T right, T bottom, T top,
T znear, T zfar, T handedness);
template <class T>
static inline Matrix<T, 4, 4> LookAtHelper(const Vector<T, 3>& at,
const Vector<T, 3>& eye,
const Vector<T, 3>& up,
T handedness);
template <class T>
static inline bool UnProjectHelper(const Vector<T, 3>& window_coord,
const Matrix<T, 4, 4>& model_view,
const Matrix<T, 4, 4>& projection,
const float window_width,
const float window_height,
Vector<T, 3>& result);
template <typename T, int rows, int columns, typename CompatibleT>
static inline Matrix<T, rows, columns> FromTypeHelper(const CompatibleT& compatible);
template <typename T, int rows, int columns, typename CompatibleT>
static inline CompatibleT ToTypeHelper(const Matrix<T, rows, columns>& m);
/// @endcond
/// @addtogroup mathfu_matrix
/// @{
/// @class Matrix
/// @brief Matrix stores a set of "rows" by "columns" elements of type T
/// and provides functions that operate on the set of elements.
///
/// @tparam T type of each element in the matrix.
/// @tparam rows Number of rows in the matrix.
/// @tparam columns Number of columns in the matrix.
template <class T, int rows, int columns>
class Matrix {
public:
/// @brief Construct a Matrix of uninitialized values.
inline Matrix() {}
/// @brief Construct a Matrix from another Matrix copying each element.
////
/// @param m Matrix that the data will be copied from.
inline Matrix(const Matrix<T, rows, columns>& m) {
MATHFU_MAT_OPERATION(data_[i] = m.data_[i]);
}
/// @brief Construct a Matrix from a single float.
///
/// @param s Scalar value used to initialize each element of the matrix.
explicit inline Matrix(const T& s) {
MATHFU_MAT_OPERATION((data_[i] = Vector<T, rows>(s)));
}
/// @brief Construct a Matrix from four floats.
///
/// @note This method only works with a 2x2 Matrix.
///
/// @param s00 Value of the first row and column.
/// @param s10 Value of the second row, first column.
/// @param s01 Value of the first row, second column.
/// @param s11 Value of the second row and column.
inline Matrix(const T& s00, const T& s10, const T& s01, const T& s11) {
MATHFU_STATIC_ASSERT(rows == 2 && columns == 2);
data_[0] = Vector<T, rows>(s00, s10);
data_[1] = Vector<T, rows>(s01, s11);
}
/// @brief Create a Matrix from nine floats.
///
/// @note This method only works with a 3x3 Matrix.
///
/// @param s00 Value of the first row and column.
/// @param s10 Value of the second row, first column.
/// @param s20 Value of the third row, first column.
/// @param s01 Value of the first row, second column.
/// @param s11 Value of the second row and column.
/// @param s21 Value of the third row, second column.
/// @param s02 Value of the first row, third column.
/// @param s12 Value of the second row, third column.
/// @param s22 Value of the third row and column.
inline Matrix(const T& s00, const T& s10, const T& s20, const T& s01,
const T& s11, const T& s21, const T& s02, const T& s12,
const T& s22) {
MATHFU_STATIC_ASSERT(rows == 3 && columns == 3);
data_[0] = Vector<T, rows>(s00, s10, s20);
data_[1] = Vector<T, rows>(s01, s11, s21);
data_[2] = Vector<T, rows>(s02, s12, s22);
}
/// @brief Creates a Matrix from twelve floats.
///
/// @note This method only works with Matrix<float, 4, 3>.
///
///
/// @param s00 Value of the first row and column.
/// @param s10 Value of the second row, first column.
/// @param s20 Value of the third row, first column.
/// @param s30 Value of the fourth row, first column.
/// @param s01 Value of the first row, second column.
/// @param s11 Value of the second row and column.
/// @param s21 Value of the third row, second column.
/// @param s31 Value of the fourth row, second column.
/// @param s02 Value of the first row, third column.
/// @param s12 Value of the second row, third column.
/// @param s22 Value of the third row and column.
/// @param s32 Value of the fourth row, third column.
inline Matrix(const T& s00, const T& s10, const T& s20, const T& s30,
const T& s01, const T& s11, const T& s21, const T& s31,
const T& s02, const T& s12, const T& s22, const T& s32) {
MATHFU_STATIC_ASSERT(rows == 4 && columns == 3);
data_[0] = Vector<T, rows>(s00, s10, s20, s30);
data_[1] = Vector<T, rows>(s01, s11, s21, s31);
data_[2] = Vector<T, rows>(s02, s12, s22, s32);
}
/// @brief Create a Matrix from sixteen floats.
///
/// @note This method only works with a 4x4 Matrix.
///
/// @param s00 Value of the first row and column.
/// @param s10 Value of the second row, first column.
/// @param s20 Value of the third row, first column.
/// @param s30 Value of the fourth row, first column.
/// @param s01 Value of the first row, second column.
/// @param s11 Value of the second row and column.
/// @param s21 Value of the third row, second column.
/// @param s31 Value of the fourth row, second column.
/// @param s02 Value of the first row, third column.
/// @param s12 Value of the second row, third column.
/// @param s22 Value of the third row and column.
/// @param s32 Value of the fourth row, third column.
/// @param s03 Value of the first row, fourth column.
/// @param s13 Value of the second row, fourth column.
/// @param s23 Value of the third row, fourth column.
/// @param s33 Value of the fourth row and column.
inline Matrix(const T& s00, const T& s10, const T& s20, const T& s30,
const T& s01, const T& s11, const T& s21, const T& s31,
const T& s02, const T& s12, const T& s22, const T& s32,
const T& s03, const T& s13, const T& s23, const T& s33) {
MATHFU_STATIC_ASSERT(rows == 4 && columns == 4);
data_[0] = Vector<T, rows>(s00, s10, s20, s30);
data_[1] = Vector<T, rows>(s01, s11, s21, s31);
data_[2] = Vector<T, rows>(s02, s12, s22, s32);
data_[3] = Vector<T, rows>(s03, s13, s23, s33);
}
/// @brief Create 4x4 Matrix from 4, 4 element vectors.
///
/// @note This method only works with a 4x4 Matrix.
///
/// @param column0 Vector used for the first column.
/// @param column1 Vector used for the second column.
/// @param column2 Vector used for the third column.
/// @param column3 Vector used for the fourth column.
inline Matrix(const Vector<T, 4>& column0, const Vector<T, 4>& column1,
const Vector<T, 4>& column2, const Vector<T, 4>& column3) {
MATHFU_STATIC_ASSERT(rows == 4 && columns == 4);
data_[0] = column0;
data_[1] = column1;
data_[2] = column2;
data_[3] = column3;
}
/// @brief Create a Matrix from the first row * column elements of an array.
///
/// @param a Array of values that the matrix will be iniitlized to.
explicit inline Matrix(const T* const a) {
MATHFU_MAT_OPERATION((data_[i] = Vector<T, rows>(&a[i * columns])));
}
/// @brief Create a Matrix from an array of "columns", "rows" element packed
/// vectors.
///
/// @param vectors Array of "columns", "rows" element packed vectors.
explicit inline Matrix(const VectorPacked<T, rows>* const vectors) {
MATHFU_MAT_OPERATION((data_[i] = Vector<T, rows>(vectors[i])));
}
/// @brief Access an element of the matrix.
///
/// @param row Index of the row to access.
/// @param column Index of the column to access.
/// @return Const reference to the element.
inline const T& operator()(const int row, const int column) const {
return data_[column][row];
}
/// @brief Access an element of the Matrix.
///
/// @param row Index of the row to access.
/// @param column Index of the column to access.
/// @return Reference to the data that can be modified by the caller.
inline T& operator()(const int row, const int column) {
return data_[column][row];
}
/// @brief Access an element of the Matrix.
///
/// @param i Index of the element to access in flattened memory. Where
/// the column accessed is i / rows and the row is i % rows.
/// @return Reference to the data that can be modified by the caller.
inline const T& operator()(const int i) const { return operator[](i); }
/// @brief Access an element of the Matrix.
///
/// @param i Index of the element to access in flattened memory. Where
/// the column accessed is i / rows and the row is i % rows.
/// @return Reference to the data that can be modified by the caller.
inline T& operator()(const int i) { return operator[](i); }
/// @brief Access an element of the Matrix.
///
/// @param i Index of the element to access in flattened memory. Where
/// the column accessed is i / rows and the row is i % rows.
/// @return Const reference to the data.
inline const T& operator[](const int i) const {
return const_cast<Matrix<T, rows, columns>*>(this)->operator[](i);
}
/// @brief Access an element of the Matrix.
///
/// @param i Index of the element to access in flattened memory. Where
/// the column accessed is i / rows and the row is i % rows.
/// @return Reference to the data that can be modified by the caller.
inline T& operator[](const int i) {
#if defined(MATHFU_COMPILE_WITH_PADDING)
// In this case Vector<T, 3> is padded, so the element offset must be
// accessed using the array operator.
if (rows == 3) {
const int row = i % rows;
const int col = i / rows;
return data_[col][row];
} else {
return reinterpret_cast<T*>(data_)[i];
}
#else
return reinterpret_cast<T*>(data_)[i];
#endif // defined(MATHFU_COMPILE_WITH_PADDING)
}
/// @brief Pack the matrix to an array of "rows" element vectors,
/// one vector per matrix column.
///
/// @param vector Array of "columns" entries to write to.
inline void Pack(VectorPacked<T, rows>* const vector) const {
MATHFU_MAT_OPERATION(GetColumn(i).Pack(&vector[i]));
}
/// @cond MATHFU_INTERNAL
/// @brief Access a column vector of the Matrix.
///
/// @param i Index of the column to access.
/// @return Reference to the data that can be modified by the caller.
inline Vector<T, rows>& GetColumn(const int i) { return data_[i]; }
/// @brief Access a column vector of the Matrix.
///
/// @param i Index of the column to access.
/// @return Const reference to the data.
inline const Vector<T, rows>& GetColumn(const int i) const {
return data_[i];
}
/// @endcond
/// @brief Negate this Matrix.
///
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator-() const {
MATHFU_MAT_OPERATOR(-data_[i]);
}
/// @brief Add a Matrix to this Matrix.
///
/// @param m Matrix to add to this Matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator+(
const Matrix<T, rows, columns>& m) const {
MATHFU_MAT_OPERATOR(data_[i] + m.data_[i]);
}
/// @brief Subtract a Matrix from this Matrix.
///
/// @param m Matrix to subtract from this Matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator-(
const Matrix<T, rows, columns>& m) const {
MATHFU_MAT_OPERATOR(data_[i] - m.data_[i]);
}
/// @brief Add a scalar to each element of this Matrix.
///
/// @param s Scalar to add to this Matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator+(const T& s) const {
MATHFU_MAT_OPERATOR(data_[i] + s);
}
/// @brief Subtract a scalar from each element of this Matrix.
///
/// @param s Scalar to subtract from this matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator-(const T& s) const {
MATHFU_MAT_OPERATOR(data_[i] - s);
}
/// @brief Multiply each element of this Matrix with a scalar.
///
/// @param s Scalar to multiply with this Matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator*(const T& s) const {
MATHFU_MAT_OPERATOR(data_[i] * s);
}
/// @brief Divide each element of this Matrix with a scalar.
///
/// @param s Scalar to divide this Matrix with.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator/(const T& s) const {
return (*this) * (1 / s);
}
/// @brief Multiply this Matrix with another Matrix.
///
/// @param m Matrix to multiply with this Matrix.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> operator*(
const Matrix<T, rows, columns>& m) const {
Matrix<T, rows, columns> result;
TimesHelper(*this, m, &result);
return result;
}
/// @brief Add a Matrix to this Matrix (in-place).
///
/// @param m Matrix to add to this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator+=(
const Matrix<T, rows, columns>& m) {
MATHFU_MAT_SELF_OPERATOR(data_[i] += m.data_[i]);
}
/// @brief Subtract a Matrix from this Matrix (in-place).
///
/// @param m Matrix to subtract from this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator-=(
const Matrix<T, rows, columns>& m) {
MATHFU_MAT_SELF_OPERATOR(data_[i] -= m.data_[i]);
}
/// @brief Add a scalar to each element of this Matrix (in-place).
///
/// @param s Scalar to add to each element of this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator+=(const T& s) {
MATHFU_MAT_SELF_OPERATOR(data_[i] += s);
}
/// @brief Subtract a scalar from each element of this Matrix (in-place).
///
/// @param s Scalar to subtract from each element of this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator-=(const T& s) {
MATHFU_MAT_SELF_OPERATOR(data_[i] -= s);
}
/// @brief Multiply each element of this Matrix with a scalar (in-place).
///
/// @param s Scalar to multiply with each element of this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator*=(const T& s) {
MATHFU_MAT_SELF_OPERATOR(data_[i] *= s);
}
/// @brief Divide each element of this Matrix by a scalar (in-place).
///
/// @param s Scalar to divide this Matrix by.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator/=(const T& s) {
return (*this) *= (1 / s);
}
/// @brief Multiply this Matrix with another Matrix (in-place).
///
/// @param m Matrix to multiply with this Matrix.
/// @return Reference to this class.
inline Matrix<T, rows, columns>& operator*=(
const Matrix<T, rows, columns>& m) {
const Matrix<T, rows, columns> copy_of_this(*this);
TimesHelper(copy_of_this, m, this);
return *this;
}
/// @brief Calculate the inverse of this Matrix.
///
/// This calculates the inverse Matrix such that
/// <code>(m * m).Inverse()</code> is the identity.
/// @return Matrix containing the result.
inline Matrix<T, rows, columns> Inverse() const {
Matrix<T, rows, columns> inverse;
InverseHelper<false>(*this, &inverse);
return inverse;
}
/// @brief Calculate the inverse of this Matrix.
///
/// This calculates the inverse Matrix such that
/// <code>(m * m).Inverse()</code> is the identity.
/// By contrast to Inverse() this returns whether the matrix is invertible.
///
/// The invertible check simply compares the calculated determinant with
/// Constants<T>::GetDeterminantThreshold() to roughly determine whether the
/// matrix is invertible. This simple check works in common cases but will
/// fail for corner cases where the matrix is a combination of huge and tiny
/// values that can't be accurately represented by the floating point
/// datatype T. More extensive checks (relative to the input values) are
/// possible but <b>far</b> more expensive, complicated and difficult to
/// test.
/// @return Whether the matrix is invertible.
inline bool InverseWithDeterminantCheck(
Matrix<T, rows, columns>* const inverse) const {
return InverseHelper<true>(*this, inverse);
}
/// @brief Calculate the transpose of this Matrix.
///
/// @return The transpose of the specified Matrix.
inline Matrix<T, columns, rows> Transpose() const {
Matrix<T, columns, rows> transpose;
MATHFU_UNROLLED_LOOP(
i, columns, MATHFU_UNROLLED_LOOP(
j, rows, transpose.GetColumn(j)[i] = GetColumn(i)[j]))
return transpose;
}
/// @brief Get the 2-dimensional translation of a 2-dimensional affine
/// transform.
///
/// @note 2-dimensional affine transforms are represented by 3x3 matrices.
/// @return Vector with the first two components of column 2 of this Matrix.
inline Vector<T, 2> TranslationVector2D() const {
MATHFU_STATIC_ASSERT(rows == 3 && columns == 3);
return Vector<T, 2>(data_[2][0], data_[2][1]);
}
/// @brief Get the 3-dimensional translation of a 3-dimensional affine
/// transform.
///
/// @note 3-dimensional affine transforms are represented by 4x4 matrices.
/// @return Vector with the first three components of column 3.
inline Vector<T, 3> TranslationVector3D() const {
MATHFU_STATIC_ASSERT(rows == 4 && columns == 4);
return Vector<T, 3>(data_[3][0], data_[3][1], data_[3][2]);
}
/// @brief Load from any byte-wise compatible external matrix.
///
/// Format should be `columns` vectors, each holding `rows` values of type T.
///
/// Use this for safe conversion from external matrix classes.
/// Often, external libraries will have their own matrix types that are,
/// byte-for-byte, exactly the same as mathfu::Matrix. This function allows
/// you to load a mathfu::Matrix from those external types, without potential
/// aliasing bugs that are caused by casting.
///
/// @note If your external type gives you access to a T*, then you can
/// equivalently use the Matrix(const T*) constructor.
///
/// @param compatible reference to a byte-wise compatible matrix structure;
/// array of columns x rows Ts.
/// @returns `compatible` loaded as a mathfu::Matrix.
template <typename CompatibleT>
static inline Matrix<T, rows, columns> FromType(const CompatibleT& compatible) {
return FromTypeHelper<T, rows, columns, CompatibleT>(compatible);
}
/// @brief Load into any byte-wise compatible external matrix.
///
/// Format should be `columns` vectors, each holding `rows` values of type T.
///
/// Use this for safe conversion to external matrix classes.
/// Often, external libraries will have their own matrix types that are,
/// byte-for-byte, exactly the same as mathfu::Matrix. This function allows
/// you to load an external type from a mathfu::Matrix, without potential
/// aliasing bugs that are caused by casting.
///
/// @param m reference to mathfu::Matrix to convert.
/// @returns CompatibleT loaded from m.
template <typename CompatibleT>
static inline CompatibleT ToType(const Matrix<T, rows, columns>& m) {
return ToTypeHelper<T, rows, columns, CompatibleT>(m);
}
/// @brief Calculate the outer product of two Vectors.
///
/// @return Matrix containing the result.
static inline Matrix<T, rows, columns> OuterProduct(
const Vector<T, rows>& v1, const Vector<T, columns>& v2) {
return OuterProductHelper(v1, v2);
}
/// @brief Calculate the hadamard / component-wise product of two matrices.
///
/// @param m1 First Matrix.
/// @param m2 Second Matrix.
/// @return Matrix containing the result.
static inline Matrix<T, rows, columns> HadamardProduct(
const Matrix<T, rows, columns>& m1, const Matrix<T, rows, columns>& m2) {
MATHFU_MAT_OPERATOR(m1[i] * m2[i]);
}
/// @brief Calculate the identity Matrix.
///
/// @return Matrix containing the result.
static inline Matrix<T, rows, columns> Identity() {
return IdentityHelper<T, rows, columns>();
}
/// @brief Create a 3x3 translation Matrix from a 2-dimensional Vector.
///
/// This matrix will have an empty or zero rotation component.
///
/// @param v Vector of size 2.
/// @return Matrix containing the result.
static inline Matrix<T, 3> FromTranslationVector(const Vector<T, 2>& v) {
return Matrix<T, 3>(1, 0, 0, 0, 1, 0, v[0], v[1], 1);
}
/// @brief Create a 4x4 translation Matrix from a 3-dimensional Vector.
///
/// This matrix will have an empty or zero rotation component.
///
/// @param v The vector of size 3.
/// @return Matrix containing the result.
static inline Matrix<T, 4> FromTranslationVector(const Vector<T, 3>& v) {
return Matrix<T, 4>(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, v[0], v[1], v[2],
1);
}
/// @brief Create a square Matrix with the diagonal component set to v.
///
/// This is an affine transform matrix, so the dimension of the vector is
/// one less than the dimension of the matrix.
///
/// @param v Vector containing components for scaling.
/// @return Matrix with v along the diagonal, and 1 in the bottom right.
static inline Matrix<T, rows> FromScaleVector(const Vector<T, rows - 1>& v) {
// TODO OPT: Use a helper function in a similar way to Identity to
// construct the matrix for the specialized cases 2, 3, 4, and only run
// this method in the general case. This will also allow you to use the
// helper methods from specialized classes like Matrix<T, 4, 4>.
Matrix<T, rows> return_matrix(Identity());
for (int i = 0; i < rows - 1; ++i) return_matrix(i, i) = v[i];
return return_matrix;
}
/// @brief Create a 4x4 Matrix from a 3x3 rotation Matrix.
///
/// This Matrix will have an empty or zero translation component.
///
/// @param m 3x3 rotation Matrix.
/// @return Matrix containing the result.
static inline Matrix<T, 4> FromRotationMatrix(const Matrix<T, 3>& m) {
return Matrix<T, 4>(m[0], m[1], m[2], 0, m[3], m[4], m[5], 0, m[6], m[7],
m[8], 0, 0, 0, 0, 1);
}
/// @brief Constructs a Matrix<float, 4> from an AffineTransform.
///
/// @param affine An AffineTransform reference to be used to construct
/// a Matrix<float, 4> by adding in the 'w' row of [0, 0, 0, 1].
static inline Matrix<T, 4> FromAffineTransform(
const Matrix<T, 4, 3>& affine) {
return Matrix<T, 4>(affine[0], affine[4], affine[8], static_cast<T>(0),
affine[1], affine[5], affine[9], static_cast<T>(0),
affine[2], affine[6], affine[10], static_cast<T>(0),
affine[3], affine[7], affine[11], static_cast<T>(1));
}
/// @brief Converts a Matrix<float, 4> into an AffineTransform.
///
/// @param m A Matrix<float, 4> reference to be converted into an
/// AffineTransform by dropping the fixed 'w' row.
///
/// @return Returns an AffineTransform that contains the essential
/// transformation data from the Matrix<float, 4>.
static inline Matrix<T, 4, 3> ToAffineTransform(const Matrix<T, 4>& m) {
return Matrix<T, 4, 3>(m[0], m[4], m[8], m[12], m[1], m[5], m[9], m[13],
m[2], m[6], m[10], m[14]);
}
/// @brief Create a 3x3 rotation Matrix from a 2D normalized directional
/// Vector around the X axis.
///
/// @param v 2D normalized directional Vector.
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationX(const Vector<T, 2>& v) {
return Matrix<T, 3>(1, 0, 0, 0, v.x, v.y, 0, -v.y, v.x);
}
/// @brief Create a 3x3 rotation Matrix from a 2D normalized directional
/// Vector around the Y axis.
///
/// @param v 2D normalized directional Vector.
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationY(const Vector<T, 2>& v) {
return Matrix<T, 3>(v.x, 0, -v.y, 0, 1, 0, v.y, 0, v.x);
}
/// @brief Create a 3x3 rotation Matrix from a 2D normalized directional
/// Vector around the Z axis.
///
/// @param v 2D normalized directional Vector.
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationZ(const Vector<T, 2>& v) {
return Matrix<T, 3>(v.x, v.y, 0, -v.y, v.x, 0, 0, 0, 1);
}
/// @brief Create a 3x3 rotation Matrix from an angle (in radians) around
/// the X axis.
///
/// @param angle Angle (in radians).
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationX(T angle) {
return RotationX(Vector<T, 2>(cosf(angle), sinf(angle)));
}
/// @brief Create a 3x3 rotation Matrix from an angle (in radians) around
/// the Y axis.
///
/// @param angle Angle (in radians).
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationY(T angle) {
return RotationY(Vector<T, 2>(cosf(angle), sinf(angle)));
}
/// @brief Create a 3x3 rotation Matrix from an angle (in radians)
/// around the Z axis.
///
/// @param angle Angle (in radians).
/// @return Matrix containing the result.
static inline Matrix<T, 3> RotationZ(T angle) {
return RotationZ(Vector<T, 2>(cosf(angle), sinf(angle)));
}
/// @brief Create a 4x4 perspective Matrix.
///
/// @param fovy Field of view.
/// @param aspect Aspect ratio.
/// @param znear Near plane location.
/// @param zfar Far plane location.
/// @param handedness 1.0f for RH, -1.0f for LH
/// @return 4x4 perspective Matrix.
static inline Matrix<T, 4, 4> Perspective(T fovy, T aspect, T znear, T zfar,
T handedness = 1) {
return PerspectiveHelper(fovy, aspect, znear, zfar, handedness);
}
/// @brief Create a 4x4 orthographic Matrix.
///
/// @param left Left extent.
/// @param right Right extent.
/// @param bottom Bottom extent.
/// @param top Top extent.
/// @param znear Near plane location.
/// @param zfar Far plane location.
/// @param handedness 1.0f for RH, -1.0f for LH
/// @return 4x4 orthographic Matrix.
static inline Matrix<T, 4, 4> Ortho(T left, T right, T bottom, T top, T znear,
T zfar, T handedness = 1) {
return OrthoHelper(left, right, bottom, top, znear, zfar, handedness);
}
/// @brief Create a 3-dimensional camera Matrix.
///
/// @param at The look-at target of the camera.
/// @param eye The position of the camera.
/// @param up The up vector in the world, for example (0, 1, 0) if the
/// y-axis is up.
/// @param handedness 1.0f for RH, -1.0f for LH.
/// @return 3-dimensional camera Matrix.
/// TODO: Change default handedness to +1 so that it matches Perspective().
static inline Matrix<T, 4, 4> LookAt(const Vector<T, 3>& at,
const Vector<T, 3>& eye,
const Vector<T, 3>& up,
T handedness = -1) {
return LookAtHelper(at, eye, up, handedness);
}
/// @brief Get the 3D position in object space from a window coordinate.
///
/// @param window_coord The window coordinate. The z value is for depth.
/// A window coordinate on the near plane will have 0 as the z value.
/// And a window coordinate on the far plane will have 1 as the z value.
/// z value should be with in [0, 1] here.
/// @param model_view The Model View matrix.
/// @param projection The projection matrix.
/// @param window_width Width of the window.
/// @param window_height Height of the window.
/// @return the mapped 3D position in object space.
static inline Vector<T, 3> UnProject(const Vector<T, 3>& window_coord,
const Matrix<T, 4, 4>& model_view,
const Matrix<T, 4, 4>& projection,
const float window_width,
const float window_height) {
Vector<T, 3> result;
UnProjectHelper(window_coord, model_view, projection, window_width,
window_height, result);
return result;
}
/// @brief Multiply a Vector by a Matrix.
///
/// @param v Vector to multiply.
/// @param m Matrix to multiply.
/// @return Matrix containing the result.
friend inline Vector<T, columns> operator*(
const Vector<T, rows>& v, const Matrix<T, rows, columns>& m) {
const int d = columns;
MATHFU_VECTOR_OPERATOR((Vector<T, rows>::DotProduct(m.data_[i], v)));
}
// Dimensions of the matrix.
/// Number of rows in the matrix.
static const int kRows = rows;
/// Number of columns in the matrix.
static const int kColumns = columns;
/// Total number of elements in the matrix.
static const int kElements = rows * columns;
MATHFU_DEFINE_CLASS_SIMD_AWARE_NEW_DELETE
private:
Vector<T, rows> data_[columns];
};
/// @}
/// @addtogroup mathfu_matrix
/// @{
/// @brief Multiply each element of a Matrix by a scalar.
///
/// @param s Scalar to multiply by.
/// @param m Matrix to multiply.
/// @return Matrix containing the result.
/// @tparam T Type of each element in the Matrix and the scalar type.
/// @tparam rows Number of rows in the matrix.
/// @tparam columns Number of columns in the matrix.
///
/// @related mathfu::Matrix
template <class T, int rows, int columns>
inline Matrix<T, rows, columns> operator*(const T& s,
const Matrix<T, columns, rows>& m) {
return m * s;
}
/// @brief Multiply a Matrix by a Vector.
///
/// @note Template specialized versions are implemented for 2x2, 3x3, and 4x4
/// matrices to increase performance. The 3x3 float is also specialized
/// to supported padded the 3-dimensional Vector in SIMD build configurations.
///
/// @param m Matrix to multiply.
/// @param v Vector to multiply.
/// @return Vector containing the result.
///
/// @related mathfu::Matrix
template <class T, int rows, int columns>
inline Vector<T, rows> operator*(const Matrix<T, rows, columns>& m,
const Vector<T, columns>& v) {
const Vector<T, rows> result(0);
int offset = 0;
for (int column = 0; column < columns; column++) {
for (int row = 0; row < rows; row++) {
result[row] += m[offset + row] * v[column];
}
offset += rows;
}
return result;
}
/// @cond MATHFU_INTERNAL
template <class T>
inline Vector<T, 2> operator*(const Matrix<T, 2, 2>& m, const Vector<T, 2>& v) {
return Vector<T, 2>(m[0] * v[0] + m[2] * v[1], m[1] * v[0] + m[3] * v[1]);
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline Vector<T, 3> operator*(const Matrix<T, 3, 3>& m, const Vector<T, 3>& v) {
return Vector<T, 3>(MATHFU_MATRIX_3X3_DOT(&m[0], v, 0, 3),
MATHFU_MATRIX_3X3_DOT(&m[0], v, 1, 3),
MATHFU_MATRIX_3X3_DOT(&m[0], v, 2, 3));
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <>
inline Vector<float, 3> operator*(const Matrix<float, 3, 3>& m,
const Vector<float, 3>& v) {
return Vector<float, 3>(
MATHFU_MATRIX_3X3_DOT(&m[0], v, 0, MATHFU_VECTOR_STRIDE_FLOATS(v)),
MATHFU_MATRIX_3X3_DOT(&m[0], v, 1, MATHFU_VECTOR_STRIDE_FLOATS(v)),
MATHFU_MATRIX_3X3_DOT(&m[0], v, 2, MATHFU_VECTOR_STRIDE_FLOATS(v)));
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline Vector<T, 4> operator*(const Matrix<T, 4, 4>& m, const Vector<T, 4>& v) {
return Vector<T, 4>(
MATHFU_MATRIX_4X4_DOT(&m[0], v, 0), MATHFU_MATRIX_4X4_DOT(&m[0], v, 1),
MATHFU_MATRIX_4X4_DOT(&m[0], v, 2), MATHFU_MATRIX_4X4_DOT(&m[0], v, 3));
}
/// @endcond
/// @brief Multiply a 4x4 Matrix by a 3-dimensional Vector.
///
/// This is provided as a convenience and assumes the vector has a fourth
/// component equal to 1.
///
/// @param m 4x4 Matrix.
/// @param v 3-dimensional Vector.
/// @return 3-dimensional Vector result.
///
/// @related mathfu::Matrix
template <class T>
inline Vector<T, 3> operator*(const Matrix<T, 4, 4>& m, const Vector<T, 3>& v) {
Vector<T, 4> v4(v[0], v[1], v[2], 1);
v4 = m * v4;
return Vector<T, 3>(v4[0] / v4[3], v4[1] / v4[3], v4[2] / v4[3]);
}
/// @cond MATHFU_INTERNAL
/// @brief Multiply a Matrix with another Matrix.
///
/// @note Template specialized versions are implemented for 2x2, 3x3, and 4x4
/// matrices to improve performance. 3x3 float is also specialized because if
/// SIMD is used the vectors of this type of length 4.
///
/// @param m1 Matrix to multiply.
/// @param m2 Matrix to multiply.
/// @param out_m Pointer to a Matrix which receives the result.
///
/// @tparam T Type of each element in the returned Matrix.
/// @tparam size1 Number of rows in the returned Matrix and columns in m1.
/// @tparam size2 Number of columns in the returned Matrix and rows in m2.
/// @tparam size3 Number of columns in m3.
template <class T, int size1, int size2, int size3>
inline void TimesHelper(const Matrix<T, size1, size2>& m1,
const Matrix<T, size2, size3>& m2,
Matrix<T, size1, size3>* out_m) {
for (int i = 0; i < size1; i++) {
for (int j = 0; j < size3; j++) {
Vector<T, size2> row;
for (int k = 0; k < size2; k++) {
row[k] = m1(i, k);
}
(*out_m)(i, j) = Vector<T, size2>::DotProduct(m2.GetColumn(j), row);
}
}
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline void TimesHelper(const Matrix<T, 2, 2>& m1, const Matrix<T, 2, 2>& m2,
Matrix<T, 2, 2>* out_m) {
Matrix<T, 2, 2>& out = *out_m;
out[0] = m1[0] * m2[0] + m1[2] * m2[1];
out[1] = m1[1] * m2[0] + m1[3] * m2[1];
out[2] = m1[0] * m2[2] + m1[2] * m2[3];
out[3] = m1[1] * m2[2] + m1[3] * m2[3];
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <typename T>
inline void TimesHelper(const Matrix<T, 3, 3>& m1, const Matrix<T, 3, 3>& m2,
Matrix<T, 3, 3>* out_m) {
Matrix<T, 3, 3>& out = *out_m;
{
Vector<T, 3> row(m1[0], m1[3], m1[6]);
out[0] = Vector<T, 3>::DotProduct(m2.GetColumn(0), row);
out[3] = Vector<T, 3>::DotProduct(m2.GetColumn(1), row);
out[6] = Vector<T, 3>::DotProduct(m2.GetColumn(2), row);
}
{
Vector<T, 3> row(m1[1], m1[4], m1[7]);
out[1] = Vector<T, 3>::DotProduct(m2.GetColumn(0), row);
out[4] = Vector<T, 3>::DotProduct(m2.GetColumn(1), row);
out[7] = Vector<T, 3>::DotProduct(m2.GetColumn(2), row);
}
{
Vector<T, 3> row(m1[2], m1[5], m1[8]);
out[2] = Vector<T, 3>::DotProduct(m2.GetColumn(0), row);
out[5] = Vector<T, 3>::DotProduct(m2.GetColumn(1), row);
out[8] = Vector<T, 3>::DotProduct(m2.GetColumn(2), row);
}
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline void TimesHelper(const Matrix<T, 4, 4>& m1, const Matrix<T, 4, 4>& m2,
Matrix<T, 4, 4>* out_m) {
Matrix<T, 4, 4>& out = *out_m;
{
Vector<T, 4> row(m1[0], m1[4], m1[8], m1[12]);
out[0] = Vector<T, 4>::DotProduct(m2.GetColumn(0), row);
out[4] = Vector<T, 4>::DotProduct(m2.GetColumn(1), row);
out[8] = Vector<T, 4>::DotProduct(m2.GetColumn(2), row);
out[12] = Vector<T, 4>::DotProduct(m2.GetColumn(3), row);
}
{
Vector<T, 4> row(m1[1], m1[5], m1[9], m1[13]);
out[1] = Vector<T, 4>::DotProduct(m2.GetColumn(0), row);
out[5] = Vector<T, 4>::DotProduct(m2.GetColumn(1), row);
out[9] = Vector<T, 4>::DotProduct(m2.GetColumn(2), row);
out[13] = Vector<T, 4>::DotProduct(m2.GetColumn(3), row);
}
{
Vector<T, 4> row(m1[2], m1[6], m1[10], m1[14]);
out[2] = Vector<T, 4>::DotProduct(m2.GetColumn(0), row);
out[6] = Vector<T, 4>::DotProduct(m2.GetColumn(1), row);
out[10] = Vector<T, 4>::DotProduct(m2.GetColumn(2), row);
out[14] = Vector<T, 4>::DotProduct(m2.GetColumn(3), row);
}
{
Vector<T, 4> row(m1[3], m1[7], m1[11], m1[15]);
out[3] = Vector<T, 4>::DotProduct(m2.GetColumn(0), row);
out[7] = Vector<T, 4>::DotProduct(m2.GetColumn(1), row);
out[11] = Vector<T, 4>::DotProduct(m2.GetColumn(2), row);
out[15] = Vector<T, 4>::DotProduct(m2.GetColumn(3), row);
}
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// @brief Compute the identity matrix.
///
/// @note There are template specializations for 2x2, 3x3, and 4x4 matrices to
/// increase performance.
///
/// @return Identity Matrix.
/// @tparam T Type of each element in the returned Matrix.
/// @tparam rows Number of rows in the returned Matrix.
/// @tparam columns Number of columns in the returned Matrix.
template <class T, int rows, int columns>
inline Matrix<T, rows, columns> IdentityHelper() {
Matrix<T, rows, columns> return_matrix(0.f);
int min_d = rows < columns ? rows : columns;
for (int i = 0; i < min_d; ++i) return_matrix(i, i) = 1;
return return_matrix;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline Matrix<T, 2, 2> IdentityHelper() {
return Matrix<T, 2, 2>(1, 0, 0, 1);
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline Matrix<T, 3, 3> IdentityHelper() {
return Matrix<T, 3, 3>(1, 0, 0, 0, 1, 0, 0, 0, 1);
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline Matrix<T, 4, 4> IdentityHelper() {
return Matrix<T, 4, 4>(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// @brief Compute the outer product of two vectors.
///
/// @note There are template specialization for 2x2, 3x3, and 4x4 matrices to
/// increase performance.
template <class T, int rows, int columns>
static inline Matrix<T, rows, columns> OuterProductHelper(
const Vector<T, rows>& v1, const Vector<T, columns>& v2) {
Matrix<T, rows, columns> result(0);
int offset = 0;
for (int column = 0; column < columns; column++) {
for (int row = 0; row < rows; row++) {
result[row + offset] = v1[row] * v2[column];
}
offset += rows;
}
return result;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
static inline Matrix<T, 2, 2> OuterProductHelper(const Vector<T, 2>& v1,
const Vector<T, 2>& v2) {
return Matrix<T, 2, 2>(v1[0] * v2[0], v1[1] * v2[0], v1[0] * v2[1],
v1[1] * v2[1]);
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
static inline Matrix<T, 3, 3> OuterProductHelper(const Vector<T, 3>& v1,
const Vector<T, 3>& v2) {
return Matrix<T, 3, 3>(v1[0] * v2[0], v1[1] * v2[0], v1[2] * v2[0],
v1[0] * v2[1], v1[1] * v2[1], v1[2] * v2[1],
v1[0] * v2[2], v1[1] * v2[2], v1[2] * v2[2]);
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
static inline Matrix<T, 4, 4> OuterProductHelper(const Vector<T, 4>& v1,
const Vector<T, 4>& v2) {
return Matrix<T, 4, 4>(
v1[0] * v2[0], v1[1] * v2[0], v1[2] * v2[0], v1[3] * v2[0], v1[0] * v2[1],
v1[1] * v2[1], v1[2] * v2[1], v1[3] * v2[1], v1[0] * v2[2], v1[1] * v2[2],
v1[2] * v2[2], v1[3] * v2[2], v1[0] * v2[3], v1[1] * v2[3], v1[2] * v2[3],
v1[3] * v2[3]);
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Struct used for template specialization for functions that
/// returns constants.
template <class T>
class Constants {
public:
/// Minimum absolute value of the determinant of an invertible matrix.
static T GetDeterminantThreshold() {
// No constant defined for the general case.
assert(false);
return 0;
}
};
/// @endcond
/// Functions that return constants for <code>float</code> values.
template <>
class Constants<float> {
public:
/// @brief Minimum absolute value of the determinant of an invertible
/// <code>float</code> Matrix.
///
/// <code>float</code> values have 23 bits of precision which is roughly
/// 1e7f, given that the final step of matrix inversion is multiplication
/// with the inverse of the determinant, the minimum value of the
/// determinant is 1e-7f before the precision too low to accurately
/// calculate the inverse.
/// @returns Minimum absolute value of the determinant of an invertible
/// <code>float</code> Matrix.
///
/// @related mathfu::Matrix::InverseWithDeterminantCheck()
static float GetDeterminantThreshold() { return 1e-7f; }
};
/// Functions that return constants for <code>double</code> values.
template <>
class Constants<double> {
public:
/// @brief Minimum absolute value of the determinant of an invertible
/// <code>double</code> Matrix.
///
/// <code>double</code> values have 46 bits of precision which is roughly
/// 1e15, given that the final step of matrix inversion is multiplication
/// with the inverse of the determinant, the minimum value of the
/// determinant is 1e-15 before the precision too low to accurately
/// calculate the inverse.
/// @returns Minimum absolute value of the determinant of an invertible
/// <code>double</code> Matrix.
///
/// @related mathfu::Matrix::InverseWithDeterminantCheck()
static double GetDeterminantThreshold() { return 1e-15; }
};
/// @cond MATHFU_INTERNAL
/// @brief Compute the inverse of a matrix.
///
/// There is template specialization for 2x2, 3x3, and 4x4 matrices to
/// increase performance. Inverse is not implemented for dense matrices that
/// are not of size 2x2, 3x3, and 4x4. If check_invertible is true the
/// determine of the matrix is compared with
/// Constants<T>::GetDeterminantThreshold() to roughly determine whether the
/// Matrix is invertible.
template <bool check_invertible, class T, int rows, int columns>
inline bool InverseHelper(const Matrix<T, rows, columns>& m,
Matrix<T, rows, columns>* const inverse) {
assert(false);
(void)m;
*inverse = T::Identity();
return false;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <bool check_invertible, class T>
inline bool InverseHelper(const Matrix<T, 2, 2>& m,
Matrix<T, 2, 2>* const inverse) {
T determinant = m[0] * m[3] - m[1] * m[2];
if (check_invertible &&
fabs(determinant) < Constants<T>::GetDeterminantThreshold()) {
return false;
}
T inverseDeterminant = 1 / determinant;
(*inverse)[0] = inverseDeterminant * m[3];
(*inverse)[1] = -inverseDeterminant * m[1];
(*inverse)[2] = -inverseDeterminant * m[2];
(*inverse)[3] = inverseDeterminant * m[0];
return true;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <bool check_invertible, class T>
inline bool InverseHelper(const Matrix<T, 3, 3>& m,
Matrix<T, 3, 3>* const inverse) {
// Find determinant of matrix.
T sub11 = m[4] * m[8] - m[5] * m[7], sub12 = -m[1] * m[8] + m[2] * m[7],
sub13 = m[1] * m[5] - m[2] * m[4];
T determinant = m[0] * sub11 + m[3] * sub12 + m[6] * sub13;
if (check_invertible &&
fabs(determinant) < Constants<T>::GetDeterminantThreshold()) {
return false;
}
// Find determinants of 2x2 submatrices for the elements of the inverse.
*inverse = Matrix<T, 3, 3>(
sub11, sub12, sub13, m[6] * m[5] - m[3] * m[8], m[0] * m[8] - m[6] * m[2],
m[3] * m[2] - m[0] * m[5], m[3] * m[7] - m[6] * m[4],
m[6] * m[1] - m[0] * m[7], m[0] * m[4] - m[3] * m[1]);
*(inverse) *= 1 / determinant;
return true;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <class T>
inline int FindLargestPivotElem(const Matrix<T, 4, 4>& m) {
Vector<T, 4> fabs_column(fabs(m[0]), fabs(m[1]), fabs(m[2]), fabs(m[3]));
if (fabs_column[0] > fabs_column[1]) {
if (fabs_column[0] > fabs_column[2]) {
if (fabs_column[0] > fabs_column[3]) {
return 0;
} else {
return 3;
}
} else if (fabs_column[2] > fabs_column[3]) {
return 2;
} else {
return 3;
}
} else if (fabs_column[1] > fabs_column[2]) {
if (fabs_column[1] > fabs_column[3]) {
return 1;
} else {
return 3;
}
} else if (fabs_column[2] > fabs_column[3]) {
return 2;
} else {
return 3;
}
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <bool check_invertible, class T>
bool InverseHelper(const Matrix<T, 4, 4>& m, Matrix<T, 4, 4>* const inverse) {
// This will find the pivot element.
int pivot_elem = FindLargestPivotElem(m);
// This will perform the pivot and find the row, column, and 3x3 submatrix
// for this pivot.
Vector<T, 3> row, column;
Matrix<T, 3> matrix;
if (pivot_elem == 0) {
row = Vector<T, 3>(m[4], m[8], m[12]);
column = Vector<T, 3>(m[1], m[2], m[3]);
matrix =
Matrix<T, 3>(m[5], m[6], m[7], m[9], m[10], m[11], m[13], m[14], m[15]);
} else if (pivot_elem == 1) {
row = Vector<T, 3>(m[5], m[9], m[13]);
column = Vector<T, 3>(m[0], m[2], m[3]);
matrix =
Matrix<T, 3>(m[4], m[6], m[7], m[8], m[10], m[11], m[12], m[14], m[15]);
} else if (pivot_elem == 2) {
row = Vector<T, 3>(m[6], m[10], m[14]);
column = Vector<T, 3>(m[0], m[1], m[3]);
matrix =
Matrix<T, 3>(m[4], m[5], m[7], m[8], m[9], m[11], m[12], m[13], m[15]);
} else {
row = Vector<T, 3>(m[7], m[11], m[15]);
column = Vector<T, 3>(m[0], m[1], m[2]);
matrix =
Matrix<T, 3>(m[4], m[5], m[6], m[8], m[9], m[10], m[12], m[13], m[14]);
}
T pivot_value = m[pivot_elem];
if (check_invertible &&
fabs(pivot_value) < Constants<T>::GetDeterminantThreshold()) {
return false;
}
// This will compute the inverse using the row, column, and 3x3 submatrix.
T inv = -1 / pivot_value;
row *= inv;
matrix += Matrix<T, 3>::OuterProduct(column, row);
Matrix<T, 3> mat_inverse;
if (!InverseHelper<check_invertible>(matrix, &mat_inverse) &&
check_invertible) {
return false;
}
Vector<T, 3> col_inverse = mat_inverse * (column * inv);
Vector<T, 3> row_inverse = row * mat_inverse;
T pivot_inverse = Vector<T, 3>::DotProduct(row, col_inverse) - inv;
if (pivot_elem == 0) {
*inverse = Matrix<T, 4, 4>(
pivot_inverse, col_inverse[0], col_inverse[1], col_inverse[2],
row_inverse[0], mat_inverse[0], mat_inverse[1], mat_inverse[2],
row_inverse[1], mat_inverse[3], mat_inverse[4], mat_inverse[5],
row_inverse[2], mat_inverse[6], mat_inverse[7], mat_inverse[8]);
} else if (pivot_elem == 1) {
*inverse = Matrix<T, 4, 4>(
row_inverse[0], mat_inverse[0], mat_inverse[1], mat_inverse[2],
pivot_inverse, col_inverse[0], col_inverse[1], col_inverse[2],
row_inverse[1], mat_inverse[3], mat_inverse[4], mat_inverse[5],
row_inverse[2], mat_inverse[6], mat_inverse[7], mat_inverse[8]);
} else if (pivot_elem == 2) {
*inverse = Matrix<T, 4, 4>(
row_inverse[0], mat_inverse[0], mat_inverse[1], mat_inverse[2],
row_inverse[1], mat_inverse[3], mat_inverse[4], mat_inverse[5],
pivot_inverse, col_inverse[0], col_inverse[1], col_inverse[2],
row_inverse[2], mat_inverse[6], mat_inverse[7], mat_inverse[8]);
} else {
*inverse = Matrix<T, 4, 4>(
row_inverse[0], mat_inverse[0], mat_inverse[1], mat_inverse[2],
row_inverse[1], mat_inverse[3], mat_inverse[4], mat_inverse[5],
row_inverse[2], mat_inverse[6], mat_inverse[7], mat_inverse[8],
pivot_inverse, col_inverse[0], col_inverse[1], col_inverse[2]);
}
return true;
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Create a 4x4 perpective matrix.
template <class T>
inline Matrix<T, 4, 4> PerspectiveHelper(T fovy, T aspect, T znear, T zfar,
T handedness) {
const T y = 1 / std::tan(fovy * static_cast<T>(.5));
const T x = y / aspect;
const T zdist = (znear - zfar);
const T zfar_per_zdist = zfar / zdist;
return Matrix<T, 4, 4>(x, 0, 0, 0, 0, y, 0, 0, 0, 0,
zfar_per_zdist * handedness, -1 * handedness, 0, 0,
2.0f * znear * zfar_per_zdist, 0);
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Create a 4x4 orthographic matrix.
template <class T>
static inline Matrix<T, 4, 4> OrthoHelper(T left, T right, T bottom, T top,
T znear, T zfar, T handedness) {
return Matrix<T, 4, 4>(static_cast<T>(2) / (right - left), 0, 0, 0, 0,
static_cast<T>(2) / (top - bottom), 0, 0, 0, 0,
-handedness * static_cast<T>(2) / (zfar - znear), 0,
-(right + left) / (right - left),
-(top + bottom) / (top - bottom),
-(zfar + znear) / (zfar - znear), static_cast<T>(1));
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Calculate the axes required to construct a 3-dimensional camera matrix that
/// looks at "at" from eye position "eye" with the up vector "up". The axes
/// are returned in a 4 element "axes" array.
template <class T>
static void LookAtHelperCalculateAxes(const Vector<T, 3>& at,
const Vector<T, 3>& eye,
const Vector<T, 3>& up, T handedness,
Vector<T, 3>* const axes) {
// Notice that y-axis is always the same regardless of handedness.
axes[2] = (at - eye).Normalized();
axes[0] = Vector<T, 3>::CrossProduct(up, axes[2]).Normalized();
axes[1] = Vector<T, 3>::CrossProduct(axes[2], axes[0]);
axes[3] = Vector<T, 3>(handedness * Vector<T, 3>::DotProduct(axes[0], eye),
-Vector<T, 3>::DotProduct(axes[1], eye),
handedness * Vector<T, 3>::DotProduct(axes[2], eye));
// Default calculation is left-handed (i.e. handedness=-1).
// Negate x and z axes for right-handed (i.e. handedness=+1) case.
const T neg = -handedness;
axes[0] *= neg;
axes[2] *= neg;
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Create a 3-dimensional camera matrix.
template <class T>
static inline Matrix<T, 4, 4> LookAtHelper(const Vector<T, 3>& at,
const Vector<T, 3>& eye,
const Vector<T, 3>& up,
T handedness) {
Vector<T, 3> axes[4];
LookAtHelperCalculateAxes(at, eye, up, handedness, axes);
const Vector<T, 4> column0(axes[0][0], axes[1][0], axes[2][0], 0);
const Vector<T, 4> column1(axes[0][1], axes[1][1], axes[2][1], 0);
const Vector<T, 4> column2(axes[0][2], axes[1][2], axes[2][2], 0);
const Vector<T, 4> column3(axes[3], 1);
return Matrix<T, 4, 4>(column0, column1, column2, column3);
}
/// @endcond
/// @cond MATHFU_INTERNAL
/// Get the 3D position in object space from a window coordinate.
template <class T>
static inline bool UnProjectHelper(const Vector<T, 3>& window_coord,
const Matrix<T, 4, 4>& model_view,
const Matrix<T, 4, 4>& projection,
const float window_width,
const float window_height,
Vector<T, 3>& result) {
if (window_coord.z < static_cast<T>(0) ||
window_coord.z > static_cast<T>(1)) {
// window_coord.z should be with in [0, 1]
// 0: near plane
// 1: far plane
return false;
}
Matrix<T, 4, 4> matrix = (projection * model_view).Inverse();
Vector<T, 4> standardized = Vector<T, 4>(
static_cast<T>(2) * (window_coord.x - window_width) / window_width +
static_cast<T>(1),
static_cast<T>(2) * (window_coord.y - window_height) / window_height +
static_cast<T>(1),
static_cast<T>(2) * window_coord.z - static_cast<T>(1),
static_cast<T>(1));
Vector<T, 4> multiply = matrix * standardized;
if (multiply.w == static_cast<T>(0)) {
return false;
}
result = multiply.xyz() / multiply.w;
return true;
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <typename T, int rows, int columns, typename CompatibleT>
static inline Matrix<T, rows, columns> FromTypeHelper(const CompatibleT& compatible) {
// C++11 is required for constructed unions.
#if __cplusplus >= 201103L
// Use a union instead of reinterpret_cast to avoid aliasing bugs.
union ConversionUnion {
ConversionUnion() {} // C++11.
CompatibleT compatible;
VectorPacked<T, rows> packed[columns];
} u;
static_assert(sizeof(u.compatible) == sizeof(u.packed), "Conversion size mismatch.");
// The read of `compatible` and write to `u.compatible` gets optimized away,
// and this becomes essentially a safe reinterpret_cast.
u.compatible = compatible;
// Call the packed vector constructor with the `compatible` data.
return Matrix<T, rows, columns>(u.packed);
#else
// Use the less-desirable memcpy technique if C++11 is not available.
// Most compilers understand memcpy deep enough to avoid replace the function
// call with a series of load/stores, which should then get optimized away,
// however in the worst case the optimize away may not happen.
// Note: Memcpy avoids aliasing bugs because it operates via unsigned char*,
// which is allowed to alias any type.
// See:
// http://stackoverflow.com/questions/15745030/type-punning-with-void-without-breaking-the-strict-aliasing-rule-in-c99
Matrix<T, rows, columns> m;
assert(sizeof(m) == sizeof(compatible));
memcpy(&m, &compatible, sizeof(m));
return m;
#endif // __cplusplus >= 201103L
}
/// @endcond
/// @cond MATHFU_INTERNAL
template <typename T, int rows, int columns, typename CompatibleT>
static inline CompatibleT ToTypeHelper(const Matrix<T, rows, columns>& m) {
// See FromTypeHelper() for comments.
#if __cplusplus >= 201103L
union ConversionUnion {
ConversionUnion() {}
CompatibleT compatible;
VectorPacked<T, rows> packed[columns];
} u;
static_assert(sizeof(u.compatible) == sizeof(u.packed), "Conversion size mismatch.");
m.Pack(u.packed);
return u.compatible;
#else
CompatibleT compatible;
assert(sizeof(m) == sizeof(compatible));
memcpy(&compatible, &m, sizeof(compatible));
return compatible;
#endif // __cplusplus >= 201103L
}
/// @endcond
/// @typedef AffineTransform
///
/// @brief A typedef representing a 4x3 float affine transformation.
/// Since the last row ('w' row) of an affine transformation is fixed,
/// this data type only includes the variable information for the transform.
typedef Matrix<float, 4, 3> AffineTransform;
/// @}
} // namespace mathfu
#ifdef _MSC_VER
#pragma warning(pop)
#endif
// Include the specializations to avoid template errors.
// See includes at bottom of vector.h for further explanation.
#include "mathfu/matrix_4x4.h"
#endif // MATHFU_MATRIX_H_
Reference in New Issue View Git Blame Copy Permalink