mirror of
https://github.com/OPM/ResInsight.git
synced 2025-02-16 18:34:54 -06:00
938 lines
33 KiB
C++
938 lines
33 KiB
C++
//##################################################################################################
|
|
//
|
|
// Custom Visualization Core library
|
|
// Copyright (C) 2011-2013 Ceetron AS
|
|
//
|
|
// This library may be used under the terms of either the GNU General Public License or
|
|
// the GNU Lesser General Public License as follows:
|
|
//
|
|
// GNU General Public License Usage
|
|
// This library is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This library is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
// FITNESS FOR A PARTICULAR PURPOSE.
|
|
//
|
|
// See the GNU General Public License at <<http://www.gnu.org/licenses/gpl.html>>
|
|
// for more details.
|
|
//
|
|
// GNU Lesser General Public License Usage
|
|
// This library is free software; you can redistribute it and/or modify
|
|
// it under the terms of the GNU Lesser General Public License as published by
|
|
// the Free Software Foundation; either version 2.1 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This library is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
// FITNESS FOR A PARTICULAR PURPOSE.
|
|
//
|
|
// See the GNU Lesser General Public License at <<http://www.gnu.org/licenses/lgpl-2.1.html>>
|
|
// for more details.
|
|
//
|
|
//##################################################################################################
|
|
|
|
|
|
namespace cvf {
|
|
|
|
|
|
|
|
//==================================================================================================
|
|
///
|
|
/// \class cvf::Matrix4
|
|
/// \ingroup Core
|
|
///
|
|
/// Matrices are stored internally as a one dimensional array for performance reasons.
|
|
///
|
|
/// The internal indices into the 1D array are as follows:
|
|
///
|
|
/// <PRE>
|
|
/// | m00 m01 m02 m03 | | 0 4 8 12 |
|
|
/// | m10 m11 m12 m13 | | 1 5 9 13 |
|
|
/// | m20 m21 m22 m23 | | 2 6 10 14 |
|
|
/// | m30 m31 m32 m33 | | 3 7 11 15 |
|
|
/// </PRE>
|
|
///
|
|
/// This is consistent with the way matrices are represented in %OpenGL.
|
|
/// To exemplify, translation values are stored in elements 12,13,14; see figure below
|
|
///
|
|
/// <PRE>
|
|
/// | 1 0 0 Tx |
|
|
/// | 0 1 0 Ty |
|
|
/// | 0 0 1 Tz |
|
|
/// | 0 0 0 1 |
|
|
/// </PRE>
|
|
///
|
|
/// From the %OpenGL red book (page 68) v' = M*v
|
|
///
|
|
/// <PRE>
|
|
/// | X'| | 1 0 0 Tx | | X |
|
|
/// | Y'| | 0 1 0 Ty | | Y |
|
|
/// | Z'| = | 0 0 1 Tz | * | Z |
|
|
/// | 1 | | 0 0 0 1 | | 1 |
|
|
/// </PRE>
|
|
///
|
|
/// Beware when porting code that uses C style double array indexing. In this case, the first
|
|
/// index given will corrspond to the columnd, eg M[3][0] = Tx, M[3][1] = Ty, M[3][2] = Tz
|
|
///
|
|
/// To ease accessing the internal 1D array in implementations, the private eij constants can be used.
|
|
/// These are consistent with the normal row column ordering, so that e02 accesses the element
|
|
/// in the first row and the third column.
|
|
///
|
|
//==================================================================================================
|
|
|
|
template<typename S> Matrix4<S> const Matrix4<S>::IDENTITY;
|
|
template<typename S> Matrix4<S> const Matrix4<S>::ZERO(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Default constructor. Initializes matrix to identity
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S>::Matrix4()
|
|
{
|
|
setIdentity();
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Copy constructor
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline Matrix4<S>::Matrix4(const Matrix4& other)
|
|
{
|
|
System::memcpy(m_v, sizeof(m_v), other.m_v, sizeof(other.m_v));
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Constructor with explicit initialization of all matrix elements.
|
|
///
|
|
/// The value of the parameter \a mrc will be placed in row r and column c of the matrix, eg m23
|
|
/// goes into row 2, column 3.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S>::Matrix4(S m00, S m01, S m02, S m03, S m10, S m11, S m12, S m13, S m20, S m21, S m22, S m23, S m30, S m31, S m32, S m33)
|
|
{
|
|
m_v[e00] = m00;
|
|
m_v[e10] = m10;
|
|
m_v[e20] = m20;
|
|
m_v[e30] = m30;
|
|
m_v[e01] = m01;
|
|
m_v[e11] = m11;
|
|
m_v[e21] = m21;
|
|
m_v[e31] = m31;
|
|
m_v[e02] = m02;
|
|
m_v[e12] = m12;
|
|
m_v[e22] = m22;
|
|
m_v[e32] = m32;
|
|
m_v[e03] = m03;
|
|
m_v[e13] = m13;
|
|
m_v[e23] = m23;
|
|
m_v[e33] = m33;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Matrix4<S>::Matrix4(const Matrix3<S>& other)
|
|
{
|
|
setFromMatrix3(other);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
template<typename T>
|
|
Matrix4<S>::Matrix4(const Matrix4<T>& other)
|
|
{
|
|
m_v[e00] = static_cast<S>(other.rowCol(0, 0));
|
|
m_v[e10] = static_cast<S>(other.rowCol(1, 0));
|
|
m_v[e20] = static_cast<S>(other.rowCol(2, 0));
|
|
m_v[e30] = static_cast<S>(other.rowCol(3, 0));
|
|
m_v[e01] = static_cast<S>(other.rowCol(0, 1));
|
|
m_v[e11] = static_cast<S>(other.rowCol(1, 1));
|
|
m_v[e21] = static_cast<S>(other.rowCol(2, 1));
|
|
m_v[e31] = static_cast<S>(other.rowCol(3, 1));
|
|
m_v[e02] = static_cast<S>(other.rowCol(0, 2));
|
|
m_v[e12] = static_cast<S>(other.rowCol(1, 2));
|
|
m_v[e22] = static_cast<S>(other.rowCol(2, 2));
|
|
m_v[e32] = static_cast<S>(other.rowCol(3, 2));
|
|
m_v[e03] = static_cast<S>(other.rowCol(0, 3));
|
|
m_v[e13] = static_cast<S>(other.rowCol(1, 3));
|
|
m_v[e23] = static_cast<S>(other.rowCol(2, 3));
|
|
m_v[e33] = static_cast<S>(other.rowCol(3, 3));
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Assignment operator
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline Matrix4<S>& Matrix4<S>::operator=(const Matrix4& obj)
|
|
{
|
|
System::memcpy(m_v, sizeof(m_v), obj.m_v, sizeof(obj.m_v));
|
|
return *this;
|
|
}
|
|
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Check if matrices are equal using exact comparisons.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
bool Matrix4<S>::equals(const Matrix4& mat) const
|
|
{
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
if (m_v[i] != mat.m_v[i]) return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Comparison operator. Checks for equality using exact comparisons.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
bool Matrix4<S>::operator==(const Matrix4& rhs) const
|
|
{
|
|
return this->equals(rhs);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Comparison operator. Checks for not equal using exact comparisons.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
bool Matrix4<S>::operator!=(const Matrix4& rhs) const
|
|
{
|
|
int i;
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
if (m_v[i] != rhs.m_v[i]) return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Multiplies this matrix M with the matrix \a mat, M = M*mat
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Matrix4<S>::multiply(const Matrix4& mat)
|
|
{
|
|
*this = (*this)*mat;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
const Matrix4<S> Matrix4<S>::operator*(const Matrix4& rhs) const
|
|
{
|
|
Matrix4 m;
|
|
m.m_v[e00] = m_v[e00] * rhs.m_v[e00] + m_v[e01] * rhs.m_v[e10] + m_v[e02] * rhs.m_v[e20] + m_v[e03] * rhs.m_v[e30];
|
|
m.m_v[e01] = m_v[e00] * rhs.m_v[e01] + m_v[e01] * rhs.m_v[e11] + m_v[e02] * rhs.m_v[e21] + m_v[e03] * rhs.m_v[e31];
|
|
m.m_v[e02] = m_v[e00] * rhs.m_v[e02] + m_v[e01] * rhs.m_v[e12] + m_v[e02] * rhs.m_v[e22] + m_v[e03] * rhs.m_v[e32];
|
|
m.m_v[e03] = m_v[e00] * rhs.m_v[e03] + m_v[e01] * rhs.m_v[e13] + m_v[e02] * rhs.m_v[e23] + m_v[e03] * rhs.m_v[e33];
|
|
|
|
m.m_v[e10] = m_v[e10] * rhs.m_v[e00] + m_v[e11] * rhs.m_v[e10] + m_v[e12] * rhs.m_v[e20] + m_v[e13] * rhs.m_v[e30];
|
|
m.m_v[e11] = m_v[e10] * rhs.m_v[e01] + m_v[e11] * rhs.m_v[e11] + m_v[e12] * rhs.m_v[e21] + m_v[e13] * rhs.m_v[e31];
|
|
m.m_v[e12] = m_v[e10] * rhs.m_v[e02] + m_v[e11] * rhs.m_v[e12] + m_v[e12] * rhs.m_v[e22] + m_v[e13] * rhs.m_v[e32];
|
|
m.m_v[e13] = m_v[e10] * rhs.m_v[e03] + m_v[e11] * rhs.m_v[e13] + m_v[e12] * rhs.m_v[e23] + m_v[e13] * rhs.m_v[e33];
|
|
|
|
m.m_v[e20] = m_v[e20] * rhs.m_v[e00] + m_v[e21] * rhs.m_v[e10] + m_v[e22] * rhs.m_v[e20] + m_v[e23] * rhs.m_v[e30];
|
|
m.m_v[e21] = m_v[e20] * rhs.m_v[e01] + m_v[e21] * rhs.m_v[e11] + m_v[e22] * rhs.m_v[e21] + m_v[e23] * rhs.m_v[e31];
|
|
m.m_v[e22] = m_v[e20] * rhs.m_v[e02] + m_v[e21] * rhs.m_v[e12] + m_v[e22] * rhs.m_v[e22] + m_v[e23] * rhs.m_v[e32];
|
|
m.m_v[e23] = m_v[e20] * rhs.m_v[e03] + m_v[e21] * rhs.m_v[e13] + m_v[e22] * rhs.m_v[e23] + m_v[e23] * rhs.m_v[e33];
|
|
|
|
m.m_v[e30] = m_v[e30] * rhs.m_v[e00] + m_v[e31] * rhs.m_v[e10] + m_v[e32] * rhs.m_v[e20] + m_v[e33] * rhs.m_v[e30];
|
|
m.m_v[e31] = m_v[e30] * rhs.m_v[e01] + m_v[e31] * rhs.m_v[e11] + m_v[e32] * rhs.m_v[e21] + m_v[e33] * rhs.m_v[e31];
|
|
m.m_v[e32] = m_v[e30] * rhs.m_v[e02] + m_v[e31] * rhs.m_v[e12] + m_v[e32] * rhs.m_v[e22] + m_v[e33] * rhs.m_v[e32];
|
|
m.m_v[e33] = m_v[e30] * rhs.m_v[e03] + m_v[e31] * rhs.m_v[e13] + m_v[e32] * rhs.m_v[e23] + m_v[e33] * rhs.m_v[e33];
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
const Vector4<S> Matrix4<S>::operator*(const Vector4<S>& rhs) const
|
|
{
|
|
return Vector4<S>(m_v[e00]*rhs.x() + m_v[e01]*rhs.y() + m_v[e02]*rhs.z() + m_v[e03]*rhs.w(),
|
|
m_v[e10]*rhs.x() + m_v[e11]*rhs.y() + m_v[e12]*rhs.z() + m_v[e13]*rhs.w(),
|
|
m_v[e20]*rhs.x() + m_v[e21]*rhs.y() + m_v[e22]*rhs.z() + m_v[e23]*rhs.w(),
|
|
m_v[e30]*rhs.x() + m_v[e31]*rhs.y() + m_v[e32]*rhs.z() + m_v[e33]*rhs.w());
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Sets this matrix to identity
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::setIdentity()
|
|
{
|
|
m_v[0] = 1;
|
|
m_v[1] = 0;
|
|
m_v[2] = 0;
|
|
m_v[3] = 0;
|
|
|
|
m_v[4] = 0;
|
|
m_v[5] = 1;
|
|
m_v[6] = 0;
|
|
m_v[7] = 0;
|
|
|
|
m_v[8] = 0;
|
|
m_v[9] = 0;
|
|
m_v[10] = 1;
|
|
m_v[11] = 0;
|
|
|
|
m_v[12] = 0;
|
|
m_v[13] = 0;
|
|
m_v[14] = 0;
|
|
m_v[15] = 1;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Check if this matrix is identity using exact comparisons
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
bool Matrix4<S>::isIdentity() const
|
|
{
|
|
if (m_v[0] != 1) return false;
|
|
if (m_v[1] != 0) return false;
|
|
if (m_v[2] != 0) return false;
|
|
if (m_v[3] != 0) return false;
|
|
|
|
if (m_v[4] != 0) return false;
|
|
if (m_v[5] != 1) return false;
|
|
if (m_v[6] != 0) return false;
|
|
if (m_v[7] != 0) return false;
|
|
|
|
if (m_v[8] != 0) return false;
|
|
if (m_v[9] != 0) return false;
|
|
if (m_v[10] != 1) return false;
|
|
if (m_v[11] != 0) return false;
|
|
|
|
if (m_v[12] != 0) return false;
|
|
if (m_v[13] != 0) return false;
|
|
if (m_v[14] != 0) return false;
|
|
if (m_v[15] != 1) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Set all matrix elements to 0
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::setZero()
|
|
{
|
|
m_v[0] = 0;
|
|
m_v[1] = 0;
|
|
m_v[2] = 0;
|
|
m_v[3] = 0;
|
|
m_v[4] = 0;
|
|
m_v[5] = 0;
|
|
m_v[6] = 0;
|
|
m_v[7] = 0;
|
|
m_v[8] = 0;
|
|
m_v[9] = 0;
|
|
m_v[10] = 0;
|
|
m_v[11] = 0;
|
|
m_v[12] = 0;
|
|
m_v[13] = 0;
|
|
m_v[14] = 0;
|
|
m_v[15] = 0;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Returns true if all elements of the matrix are 0. Uses exact comparison.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
bool Matrix4<S>::isZero() const
|
|
{
|
|
if (m_v[0] != 0) return false;
|
|
if (m_v[1] != 0) return false;
|
|
if (m_v[2] != 0) return false;
|
|
if (m_v[3] != 0) return false;
|
|
|
|
if (m_v[4] != 0) return false;
|
|
if (m_v[5] != 0) return false;
|
|
if (m_v[6] != 0) return false;
|
|
if (m_v[7] != 0) return false;
|
|
|
|
if (m_v[8] != 0) return false;
|
|
if (m_v[9] != 0) return false;
|
|
if (m_v[10] != 0) return false;
|
|
if (m_v[11] != 0) return false;
|
|
|
|
if (m_v[12] != 0) return false;
|
|
if (m_v[13] != 0) return false;
|
|
if (m_v[14] != 0) return false;
|
|
if (m_v[15] != 0) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Set the matrix element at the specified row and column
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline void Matrix4<S>::setRowCol(int row, int col, S value)
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
CVF_TIGHT_ASSERT(col >= 0 && col < 4);
|
|
|
|
m_v[4*col + row] = value;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Get the matrix element at the specified row and column
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline S Matrix4<S>::rowCol(int row, int col) const
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
CVF_TIGHT_ASSERT(col >= 0 && col < 4);
|
|
|
|
return m_v[4*col + row];
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Get modifiable reference to the the matrix element at the specified row and column
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline S& Matrix4<S>::operator()(int row, int col)
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
CVF_TIGHT_ASSERT(col >= 0 && col < 4);
|
|
|
|
return m_v[4*col + row];
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Get the matrix element at the specified row and column
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
inline S Matrix4<S>::operator()(int row, int col) const
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
CVF_TIGHT_ASSERT(col >= 0 && col < 4);
|
|
|
|
return m_v[4*col + row];
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::setRow(int row, const Vector4<S>& vector)
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
|
|
m_v[row] = vector.x();
|
|
m_v[row + 4] = vector.y();
|
|
m_v[row + 8] = vector.z();
|
|
m_v[row + 12] = vector.w();
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Vector4<S> Matrix4<S>::row(int row) const
|
|
{
|
|
CVF_TIGHT_ASSERT(row >= 0 && row < 4);
|
|
|
|
return Vector4<S>(m_v[row], m_v[row + 4], m_v[row + 8], m_v[row + 12]);
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::setCol(int column, const Vector4<S>& vector)
|
|
{
|
|
CVF_TIGHT_ASSERT(column >= 0 && column < 4);
|
|
|
|
m_v[4*column] = vector.x();
|
|
m_v[4*column + 1] = vector.y();
|
|
m_v[4*column + 2] = vector.z();
|
|
m_v[4*column + 3] = vector.w();
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Vector4<S> Matrix4<S>::col(int column) const
|
|
{
|
|
CVF_TIGHT_ASSERT(column >= 0 && column < 4);
|
|
|
|
return Vector4<S>(m_v[4*column], m_v[4*column + 1], m_v[4*column + 2], m_v[4*column + 3]);
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
template<typename T>
|
|
void Matrix4<S>::set(const Matrix4<T>& mat)
|
|
{
|
|
const T* matPtr = mat.ptr();
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
m_v[i] = static_cast<S>(matPtr[i]);
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Extracts the translation part of the transformation matrix
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Vector3<S> Matrix4<S>::translation() const
|
|
{
|
|
return Vector3<S>(m_v[e03], m_v[e13], m_v[e23]);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Sets the translation transformation part of the matrix
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::setTranslation(const Vector3<S>& trans)
|
|
{
|
|
m_v[e03] = trans.x();
|
|
m_v[e13] = trans.y();
|
|
m_v[e23] = trans.z();
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Adds translation by pre-multiplying the matrix with a matrix containing the specified translation
|
|
///
|
|
/// Adds translation to this (transformation) matrix by pre-multiplying the current matrix M with
|
|
/// a matrix T containing only the specified translation.
|
|
/// Calling this function has the effect of doing the multiplication M' = T x M
|
|
///
|
|
/// \param trans Specifies the X, Y and Z components of the translation.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::translatePreMultiply(const Vector3<S>& trans)
|
|
{
|
|
m_v[e00] += trans.x()*m_v[e30];
|
|
m_v[e01] += trans.x()*m_v[e31];
|
|
m_v[e02] += trans.x()*m_v[e32];
|
|
m_v[e03] += trans.x()*m_v[e33];
|
|
|
|
m_v[e10] += trans.y()*m_v[e30];
|
|
m_v[e11] += trans.y()*m_v[e31];
|
|
m_v[e12] += trans.y()*m_v[e32];
|
|
m_v[e13] += trans.y()*m_v[e33];
|
|
|
|
m_v[e20] += trans.z()*m_v[e30];
|
|
m_v[e21] += trans.z()*m_v[e31];
|
|
m_v[e22] += trans.z()*m_v[e32];
|
|
m_v[e23] += trans.z()*m_v[e33];
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Adds translation by post-multiplying the matrix with a matrix containing the specified translation
|
|
///
|
|
/// Adds translation to this (transformation) matrix by post-multiplying the current matrix M with
|
|
/// a matrix T containing only the specified translation.
|
|
/// Calling this function has the effect of doing the multiplication M' = M x T
|
|
///
|
|
/// \param trans Specifies the X, Y and Z coordinates of the translation.
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::translatePostMultiply(const Vector3<S>& trans)
|
|
{
|
|
m_v[e03] += trans.x()*m_v[e00] + trans.y()*m_v[e01] + trans.z()*m_v[e02];
|
|
m_v[e13] += trans.x()*m_v[e10] + trans.y()*m_v[e11] + trans.z()*m_v[e12];
|
|
m_v[e23] += trans.x()*m_v[e20] + trans.y()*m_v[e21] + trans.z()*m_v[e22];
|
|
m_v[e33] += trans.x()*m_v[e30] + trans.y()*m_v[e31] + trans.z()*m_v[e32];
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
bool Matrix4<S>::invert()
|
|
{
|
|
// Use double internally here
|
|
const double m00 = static_cast<double>(m_v[e00]);
|
|
const double m10 = static_cast<double>(m_v[e10]);
|
|
const double m20 = static_cast<double>(m_v[e20]);
|
|
const double m30 = static_cast<double>(m_v[e30]);
|
|
const double m01 = static_cast<double>(m_v[e01]);
|
|
const double m11 = static_cast<double>(m_v[e11]);
|
|
const double m21 = static_cast<double>(m_v[e21]);
|
|
const double m31 = static_cast<double>(m_v[e31]);
|
|
const double m02 = static_cast<double>(m_v[e02]);
|
|
const double m12 = static_cast<double>(m_v[e12]);
|
|
const double m22 = static_cast<double>(m_v[e22]);
|
|
const double m32 = static_cast<double>(m_v[e32]);
|
|
const double m03 = static_cast<double>(m_v[e03]);
|
|
const double m13 = static_cast<double>(m_v[e13]);
|
|
const double m23 = static_cast<double>(m_v[e23]);
|
|
const double m33 = static_cast<double>(m_v[e33]);
|
|
|
|
const double s0 = m00 * m11 - m01 * m10;
|
|
const double s1 = m00 * m12 - m02 * m10;
|
|
const double s2 = m00 * m13 - m03 * m10;
|
|
const double s3 = m01 * m12 - m02 * m11;
|
|
const double s4 = m01 * m13 - m03 * m11;
|
|
const double s5 = m02 * m13 - m03 * m12;
|
|
|
|
const double c5 = m22 * m33 - m23 * m32;
|
|
const double c4 = m21 * m33 - m23 * m31;
|
|
const double c3 = m21 * m32 - m22 * m31;
|
|
const double c2 = m20 * m33 - m23 * m30;
|
|
const double c1 = m20 * m32 - m22 * m30;
|
|
const double c0 = m20 * m31 - m21 * m30;
|
|
|
|
const double det = s0*c5 - s1*c4 + s2*c3 + s3*c2 - s4*c1 + s5*c0;
|
|
|
|
if (Math::abs(det) > std::numeric_limits<double>::epsilon())
|
|
{
|
|
const double invDet = 1/det;
|
|
|
|
m_v[e00] = static_cast<S>( ( m11 * c5 - m12 * c4 + m13 * c3 ) * invDet );
|
|
m_v[e10] = static_cast<S>( ( - m10 * c5 + m12 * c2 - m13 * c1 ) * invDet );
|
|
m_v[e20] = static_cast<S>( ( m10 * c4 - m11 * c2 + m13 * c0 ) * invDet );
|
|
m_v[e30] = static_cast<S>( ( - m10 * c3 + m11 * c1 - m12 * c0 ) * invDet );
|
|
m_v[e01] = static_cast<S>( ( - m01 * c5 + m02 * c4 - m03 * c3 ) * invDet );
|
|
m_v[e11] = static_cast<S>( ( m00 * c5 - m02 * c2 + m03 * c1 ) * invDet );
|
|
m_v[e21] = static_cast<S>( ( - m00 * c4 + m01 * c2 - m03 * c0 ) * invDet );
|
|
m_v[e31] = static_cast<S>( ( m00 * c3 - m01 * c1 + m02 * c0 ) * invDet );
|
|
m_v[e02] = static_cast<S>( ( m31 * s5 - m32 * s4 + m33 * s3 ) * invDet );
|
|
m_v[e12] = static_cast<S>( ( - m30 * s5 + m32 * s2 - m33 * s1 ) * invDet );
|
|
m_v[e22] = static_cast<S>( ( m30 * s4 - m31 * s2 + m33 * s0 ) * invDet );
|
|
m_v[e32] = static_cast<S>( ( - m30 * s3 + m31 * s1 - m32 * s0 ) * invDet );
|
|
m_v[e03] = static_cast<S>( ( - m21 * s5 + m22 * s4 - m23 * s3 ) * invDet );
|
|
m_v[e13] = static_cast<S>( ( m20 * s5 - m22 * s2 + m23 * s1 ) * invDet );
|
|
m_v[e23] = static_cast<S>( ( - m20 * s4 + m21 * s2 - m23 * s0 ) * invDet );
|
|
m_v[e33] = static_cast<S>( ( m20 * s3 - m21 * s1 + m22 * s0 ) * invDet );
|
|
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
const Matrix4<S> Matrix4<S>::getInverted(bool* pInvertible) const
|
|
{
|
|
Matrix4 m(*this);
|
|
if (m.invert())
|
|
{
|
|
if (pInvertible) *pInvertible = true;
|
|
}
|
|
else
|
|
{
|
|
if (pInvertible) *pInvertible = false;
|
|
m.setZero();
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
S Matrix4<S>::determinant() const
|
|
{
|
|
// Consider using double precision for the intermediate calculations here
|
|
// Haven't seen any direct need for it yet, but good old GLview API did that
|
|
const S a0 = m_v[e00]*m_v[e11] - m_v[e01]*m_v[e10];
|
|
const S a1 = m_v[e00]*m_v[e12] - m_v[e02]*m_v[e10];
|
|
const S a2 = m_v[e00]*m_v[e13] - m_v[e03]*m_v[e10];
|
|
const S a3 = m_v[e01]*m_v[e12] - m_v[e02]*m_v[e11];
|
|
const S a4 = m_v[e01]*m_v[e13] - m_v[e03]*m_v[e11];
|
|
const S a5 = m_v[e02]*m_v[e13] - m_v[e03]*m_v[e12];
|
|
|
|
const S b0 = m_v[e20]*m_v[e31] - m_v[e21]*m_v[e30];
|
|
const S b1 = m_v[e20]*m_v[e32] - m_v[e22]*m_v[e30];
|
|
const S b2 = m_v[e20]*m_v[e33] - m_v[e23]*m_v[e30];
|
|
const S b3 = m_v[e21]*m_v[e32] - m_v[e22]*m_v[e31];
|
|
const S b4 = m_v[e21]*m_v[e33] - m_v[e23]*m_v[e31];
|
|
const S b5 = m_v[e22]*m_v[e33] - m_v[e23]*m_v[e32];
|
|
|
|
return (a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0);
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
void Matrix4<S>::transpose()
|
|
{
|
|
S tmp;
|
|
int i, j;
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
for (j = i + 1; j < 4; j++)
|
|
{
|
|
tmp = m_v[j + 4*i];
|
|
m_v[j + 4*i] = m_v[4*j + i];
|
|
m_v[4*j + i] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
const Matrix4<S> Matrix4<S>::getTransposed() const
|
|
{
|
|
Matrix4 m(*this);
|
|
m.transpose();
|
|
return m;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Matrix4<S>::setFromMatrix3(const Matrix3<S>& mat3)
|
|
{
|
|
m_v[e00] = mat3.rowCol(0, 0);
|
|
m_v[e01] = mat3.rowCol(0, 1);
|
|
m_v[e02] = mat3.rowCol(0, 2);
|
|
m_v[e03] = 0;
|
|
m_v[e10] = mat3.rowCol(1, 0);
|
|
m_v[e11] = mat3.rowCol(1, 1);
|
|
m_v[e12] = mat3.rowCol(1, 2);
|
|
m_v[e13] = 0;
|
|
m_v[e20] = mat3.rowCol(2, 0);
|
|
m_v[e21] = mat3.rowCol(2, 1);
|
|
m_v[e22] = mat3.rowCol(2, 2);
|
|
m_v[e23] = 0;
|
|
m_v[e30] = 0;
|
|
m_v[e31] = 0;
|
|
m_v[e32] = 0;
|
|
m_v[e33] = 1;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Matrix3<S> Matrix4<S>::toMatrix3() const
|
|
{
|
|
return Matrix3<S>(m_v[e00], m_v[e01], m_v[e02],
|
|
m_v[e10], m_v[e11], m_v[e12],
|
|
m_v[e20], m_v[e21], m_v[e22]);
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Matrix4<S>::toMatrix3(Matrix3<S>* mat3) const
|
|
{
|
|
CVF_ASSERT(mat3);
|
|
mat3->setRowCol(0, 0, m_v[e00]);
|
|
mat3->setRowCol(0, 1, m_v[e01]);
|
|
mat3->setRowCol(0, 2, m_v[e02]);
|
|
mat3->setRowCol(1, 0, m_v[e10]);
|
|
mat3->setRowCol(1, 1, m_v[e11]);
|
|
mat3->setRowCol(1, 2, m_v[e12]);
|
|
mat3->setRowCol(2, 0, m_v[e20]);
|
|
mat3->setRowCol(2, 1, m_v[e21]);
|
|
mat3->setRowCol(2, 2, m_v[e22]);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
const S* Matrix4<S>::ptr() const
|
|
{
|
|
return m_v;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Static member function that creates a transformation matrix containing only translation
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S> Matrix4<S>::fromTranslation(const Vector3<S>& trans)
|
|
{
|
|
return Matrix4(1, 0, 0, trans.x(),
|
|
0, 1, 0, trans.y(),
|
|
0, 0, 1, trans.z(),
|
|
0, 0, 0, 1);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Static member function that creates a transformation matrix containing only scaling
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S> Matrix4<S>::fromScaling(const Vector3<S>& scale)
|
|
{
|
|
return Matrix4(scale.x(), 0, 0, 0,
|
|
0, scale.y(), 0, 0,
|
|
0, 0, scale.z(), 0,
|
|
0, 0, 0, 1);
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
///
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S> Matrix4<S>::fromRotation(Vector3<S> axis, S angle)
|
|
{
|
|
axis.normalize();
|
|
|
|
S rcos = Math::cos(angle);
|
|
S rsin = Math::sin(angle);
|
|
|
|
Matrix4 m;
|
|
m.m_v[e00] = rcos + axis.x()*axis.x()*(1 - rcos);
|
|
m.m_v[e10] = axis.z() * rsin + axis.y()*axis.x()*(1 - rcos);
|
|
m.m_v[e20] = -axis.y() * rsin + axis.z()*axis.x()*(1 - rcos);
|
|
m.m_v[e01] = -axis.z() * rsin + axis.x()*axis.y()*(1 - rcos);
|
|
m.m_v[e11] = rcos + axis.y()*axis.y()*(1 - rcos);
|
|
m.m_v[e21] = axis.x() * rsin + axis.z()*axis.y()*(1 - rcos);
|
|
m.m_v[e02] = axis.y() * rsin + axis.x()*axis.z()*(1 - rcos);
|
|
m.m_v[e12] = -axis.x() * rsin + axis.y()*axis.z()*(1 - rcos);
|
|
m.m_v[e22] = rcos + axis.z()*axis.z()*(1 - rcos);
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------------------------------
|
|
/// Create a rotation matrix that will align the global X, Y and Z axes with the specified axes.
|
|
///
|
|
/// Note that at least one axis must be specified and all specified axes must be normalized.
|
|
/// If two or three axes are specified, they must be orthogonal to each other.
|
|
///
|
|
/// \param xAxis Orientation of x axis
|
|
/// \param yAxis Orientation of y axis
|
|
/// \param zAxis Orientation of z axis
|
|
//----------------------------------------------------------------------------------------------------
|
|
template <typename S>
|
|
Matrix4<S> Matrix4<S>::fromCoordSystemAxes(const Vector3<S>* xAxis, const Vector3<S>* yAxis, const Vector3<S>* zAxis)
|
|
{
|
|
Vector3<S> x(0, 0, 0);
|
|
Vector3<S> y(0, 0, 0);
|
|
Vector3<S> z(0, 0, 0);
|
|
|
|
if (xAxis && yAxis && zAxis)
|
|
{
|
|
x = *xAxis;
|
|
y = *yAxis;
|
|
z = *zAxis;
|
|
}
|
|
else if (xAxis && yAxis)
|
|
{
|
|
x = *xAxis;
|
|
y = *yAxis;
|
|
z = x ^ y;
|
|
}
|
|
else if (xAxis && zAxis)
|
|
{
|
|
x = *xAxis;
|
|
z = *zAxis;
|
|
y = z ^ x;
|
|
}
|
|
else if (yAxis && zAxis)
|
|
{
|
|
y = *yAxis;
|
|
z = *zAxis;
|
|
x = y ^ z;
|
|
}
|
|
else if (xAxis)
|
|
{
|
|
xAxis->createOrthonormalBasis(0, &x, &y, &z);
|
|
}
|
|
else if (yAxis)
|
|
{
|
|
yAxis->createOrthonormalBasis(1, &x, &y, &z);
|
|
}
|
|
else if (zAxis)
|
|
{
|
|
zAxis->createOrthonormalBasis(2, &x, &y, &z);
|
|
}
|
|
else
|
|
{
|
|
CVF_FAIL_MSG("Must specify at least one axis");
|
|
}
|
|
|
|
Matrix4 m;
|
|
m.setIdentity();
|
|
m.setCol(0, Vector4<S>(x, 0));
|
|
m.setCol(1, Vector4<S>(y, 0));
|
|
m.setCol(2, Vector4<S>(z, 0));
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Post multiplication: matrix * column vector
|
|
///
|
|
/// v' = M*v
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
cvf::Vector4<S> operator*(const cvf::Matrix4<S>& m, const cvf::Vector4<S>& v)
|
|
{
|
|
cvf::Vector4<S> vec(
|
|
v.x()*m.rowCol(0,0) + v.y()*m.rowCol(0,1) + v.z()*m.rowCol(0,2) + v.w()*m.rowCol(0,3),
|
|
v.x()*m.rowCol(1,0) + v.y()*m.rowCol(1,1) + v.z()*m.rowCol(1,2) + v.w()*m.rowCol(1,3),
|
|
v.x()*m.rowCol(2,0) + v.y()*m.rowCol(2,1) + v.z()*m.rowCol(2,2) + v.w()*m.rowCol(2,3),
|
|
v.x()*m.rowCol(3,0) + v.y()*m.rowCol(3,1) + v.z()*m.rowCol(3,2) + v.w()*m.rowCol(3,3)
|
|
);
|
|
|
|
return vec;
|
|
}
|
|
|
|
}
|
|
|