mirror of
https://github.com/OPM/ResInsight.git
synced 2025-01-21 22:13:25 -06:00
396 lines
12 KiB
C++
396 lines
12 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::Quat
|
|
/// \ingroup Core
|
|
///
|
|
/// Templated quaternion
|
|
///
|
|
//==================================================================================================
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Default constructor, initializes all elements to 0
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S>::Quat()
|
|
: m_x(0),
|
|
m_y(0),
|
|
m_z(0),
|
|
m_w(0)
|
|
{
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Constructor with initialization
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S>::Quat(S x, S y, S z, S w)
|
|
: m_x(x),
|
|
m_y(y),
|
|
m_z(z),
|
|
m_w(w)
|
|
{
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Copy constructor
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S>::Quat(const Quat& other)
|
|
: m_x(other.m_x),
|
|
m_y(other.m_y),
|
|
m_z(other.m_z),
|
|
m_w(other.m_w)
|
|
{
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Conversion constructor for explicit conversion
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
template<typename T>
|
|
Quat<S>::Quat(const T& other)
|
|
{
|
|
m_x = static_cast<S>(other.x());
|
|
m_y = static_cast<S>(other.y());
|
|
m_z = static_cast<S>(other.z());
|
|
m_w = static_cast<S>(other.w());
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Assignment operator
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S>& Quat<S>::operator=(const Quat& rhs)
|
|
{
|
|
m_x = rhs.m_x;
|
|
m_y = rhs.m_y;
|
|
m_z = rhs.m_z;
|
|
m_w = rhs.m_w;
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
bool Quat<S>::operator==(const Quat& rhs) const
|
|
{
|
|
if (m_x != rhs.m_x) return false;
|
|
if (m_y != rhs.m_y) return false;
|
|
if (m_z != rhs.m_z) return false;
|
|
if (m_w != rhs.m_w) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
bool Quat<S>::operator!=(const Quat& rhs) const
|
|
{
|
|
if (m_x != rhs.m_x) return true;
|
|
if (m_y != rhs.m_y) return true;
|
|
if (m_z != rhs.m_z) return true;
|
|
if (m_w != rhs.m_w) return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Quat<S>::set(S x, S y, S z, S w)
|
|
{
|
|
m_x = x;
|
|
m_y = y;
|
|
m_z = z;
|
|
m_w = w;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Normalize the quaternion
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
bool Quat<S>::normalize()
|
|
{
|
|
// Quaternions always obey: a^2 + b^2 + c^2 + d^2 = 1.0.
|
|
// If they don't add up to 1.0, dividing by their magnitude will re-normalize them.
|
|
|
|
S len = Math::sqrt(m_x*m_x + m_y*m_y + m_z*m_z + m_w*m_w);
|
|
|
|
if (len > 0)
|
|
{
|
|
// Pre-compute 1/length and do multiplication instead of division
|
|
S oneOverLen = static_cast<S>(1.0/len);
|
|
m_x *= oneOverLen;
|
|
m_y *= oneOverLen;
|
|
m_z *= oneOverLen;
|
|
m_w *= oneOverLen;
|
|
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
// Might be NaN, so set it to zero
|
|
m_x = 0;
|
|
m_y = 0;
|
|
m_z = 0;
|
|
m_w = 0;
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Sets the quaternion from a rotation axis and an angle (in radians)
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Quat<S>::setFromAxisAngle(Vector3<S> rotationAxis, S angle)
|
|
{
|
|
*this = Quat::fromAxisAngle(rotationAxis, angle);
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Extracts the rotation in this quaternion as an axis and an angle (in radians)
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
void Quat<S>::toAxisAngle(Vector3<S>* rotationAxis, S* angle) const
|
|
{
|
|
// From matrix an quaternion FAQ
|
|
|
|
CVF_ASSERT(rotationAxis);
|
|
CVF_ASSERT(angle);
|
|
|
|
// Need a normalized quaternion
|
|
Quat nQ(*this);
|
|
nQ.normalize();
|
|
|
|
// Clamp to acos' input domain
|
|
S cos_a = Math::clamp(nQ.m_w, static_cast<S>(-1), static_cast<S>(1));
|
|
*angle = 2*Math::acos(cos_a);
|
|
|
|
S sin_a = Math::sqrt(1 - cos_a*cos_a);
|
|
if (Math::abs(sin_a) < 0.0005) sin_a = 1;
|
|
|
|
rotationAxis->x() = nQ.m_x/sin_a;
|
|
rotationAxis->y() = nQ.m_y/sin_a;
|
|
rotationAxis->z() = nQ.m_z/sin_a;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Constructs a rotation matrix from this quaternion
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Matrix3<S> Quat<S>::toMatrix3() const
|
|
{
|
|
// Based on Quaternions paper by Ken Shoemake
|
|
// The current version works even if quaternion isn't normalized
|
|
// If we assume that the quaternion is normalized, this function can be made simpler/quicker
|
|
|
|
S Nq = m_x*m_x + m_y*m_y + m_z*m_z + m_w*m_w;
|
|
S s = (Nq > 0) ? (2/Nq) : 0;
|
|
|
|
S xs = m_x*s;
|
|
S ys = m_y*s;
|
|
S zs = m_z*s;
|
|
|
|
S wx = m_w*xs;
|
|
S wy = m_w*ys;
|
|
S wz = m_w*zs;
|
|
S xx = m_x*xs;
|
|
S xy = m_x*ys;
|
|
S xz = m_x*zs;
|
|
S yy = m_y*ys;
|
|
S yz = m_y*zs;
|
|
S zz = m_z*zs;
|
|
|
|
|
|
// Default constructor initializes to identity
|
|
Matrix3<S> m;
|
|
m.setRowCol( 0, 0, 1 - (yy + zz));
|
|
m.setRowCol( 0, 1, xy - wz );
|
|
m.setRowCol( 0, 2, xz + wy );
|
|
|
|
m.setRowCol( 1, 0, xy + wz );
|
|
m.setRowCol( 1, 1, 1 - (xx + zz));
|
|
m.setRowCol( 1, 2, yz - wx );
|
|
|
|
m.setRowCol( 2, 0, xz - wy );
|
|
m.setRowCol( 2, 1, yz + wx );
|
|
m.setRowCol( 2, 2, 1 - (xx + yy));
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Constructs a rotation matrix from this quaternion
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Matrix4<S> Quat<S>::toMatrix4() const
|
|
{
|
|
// Create via the Matrix3 version
|
|
return Matrix4<S>(toMatrix3());
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Creates a quaternion from specified rotation axis and angle (in radians)
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S> Quat<S>::fromAxisAngle(Vector3<S> rotationAxis, S angle)
|
|
{
|
|
// From matrix an quaternion FAQ
|
|
|
|
rotationAxis.normalize();
|
|
|
|
S halfAngle = static_cast<S>(angle/2.0);
|
|
S sinHalfAngle = Math::sin(halfAngle);
|
|
|
|
Quat q;
|
|
q.m_x = rotationAxis.x()*sinHalfAngle;;
|
|
q.m_y = rotationAxis.y()*sinHalfAngle;
|
|
q.m_z = rotationAxis.z()*sinHalfAngle;
|
|
q.m_w = Math::cos(halfAngle);
|
|
|
|
return q;
|
|
}
|
|
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
/// Creates a quaternion from the passed rotation matrix
|
|
//--------------------------------------------------------------------------------------------------
|
|
template<typename S>
|
|
Quat<S> Quat<S>::fromRotationMatrix(const Matrix4<S>& rotMat)
|
|
{
|
|
const S m00 = rotMat.rowCol(0, 0);
|
|
const S m01 = rotMat.rowCol(0, 1);
|
|
const S m02 = rotMat.rowCol(0, 2);
|
|
const S m10 = rotMat.rowCol(1, 0);
|
|
const S m11 = rotMat.rowCol(1, 1);
|
|
const S m12 = rotMat.rowCol(1, 2);
|
|
const S m20 = rotMat.rowCol(2, 0);
|
|
const S m21 = rotMat.rowCol(2, 1);
|
|
const S m22 = rotMat.rowCol(2, 2);
|
|
|
|
|
|
// If the trace of the matrix is greater than zero, then perform an "instant" calculation.
|
|
S trace = m00 + m11 + m22;
|
|
|
|
Quat q(0, 0, 0, 1);
|
|
|
|
// Try and test against a very small value instead of 0 here
|
|
// The FAQ suggests testing against ( T > 0.00000001 ) to avoid large distortions
|
|
if (trace > 0.00000001)
|
|
{
|
|
S s = Math::sqrt(trace + 1)*2;
|
|
|
|
q.m_x = (m21 - m12)/s;
|
|
q.m_y = (m02 - m20)/s;
|
|
q.m_z = (m10 - m01)/s;
|
|
q.m_w = static_cast<S>(0.25*s);
|
|
}
|
|
else
|
|
{
|
|
// If the trace of the matrix is equal to zero then identify
|
|
// which major diagonal element has the greatest value.
|
|
int iDominantColumn = 0;
|
|
if (m11 > m00) iDominantColumn = 1;
|
|
if (m22 > m11) iDominantColumn = 2;
|
|
|
|
if (iDominantColumn == 0)
|
|
{
|
|
S s = Math::sqrt(1 + m00 - m11 - m22)*2;
|
|
if (s > 0)
|
|
{
|
|
q.m_x = static_cast<S>(0.25*s);
|
|
q.m_y = (m10 + m01)/s;
|
|
q.m_z = (m20 + m02)/s;
|
|
q.m_w = (m21 - m12)/s;
|
|
}
|
|
}
|
|
else if (iDominantColumn == 1)
|
|
{
|
|
S s = Math::sqrt(1 + m11 - m00 - m22)*2;
|
|
if (s > 0)
|
|
{
|
|
q.m_x = (m10 + m01)/s;
|
|
q.m_y = static_cast<S>(0.25*s);
|
|
q.m_z = (m21 + m12)/s;
|
|
q.m_w = (m02 - m20)/s;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
S s = Math::sqrt(1 + m22 - m00 - m11)*2;
|
|
if (s > 0)
|
|
{
|
|
q.m_x = (m20 + m02)/s;
|
|
q.m_y = (m21 + m12)/s;
|
|
q.m_z = static_cast<S>(0.25*s);
|
|
q.m_w = (m10 - m01)/s;
|
|
}
|
|
}
|
|
}
|
|
|
|
return q;
|
|
}
|
|
|
|
} // namespace cvf
|