You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
632 lines
16 KiB
Plaintext
632 lines
16 KiB
Plaintext
/*=========================================================================
|
|
|
|
Program: Visualization Toolkit
|
|
Module: vtkQuaternion.txx
|
|
|
|
Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
|
|
All rights reserved.
|
|
See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
|
|
|
|
This software is distributed WITHOUT ANY WARRANTY; without even
|
|
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
|
|
PURPOSE. See the above copyright notice for more information.
|
|
|
|
=========================================================================*/
|
|
|
|
#include "vtkQuaternion.h"
|
|
|
|
#ifndef vtkQuaternion_txx
|
|
#define vtkQuaternion_txx
|
|
|
|
#include "vtkMath.h"
|
|
|
|
#include <cmath>
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T>::vtkQuaternion()
|
|
{
|
|
this->ToIdentity();
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T>::vtkQuaternion(const T& w, const T& x, const T& y, const T& z)
|
|
{
|
|
this->Data[0] = w;
|
|
this->Data[1] = x;
|
|
this->Data[2] = y;
|
|
this->Data[3] = z;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
T vtkQuaternion<T>::SquaredNorm() const
|
|
{
|
|
T result = 0.0;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
result += this->Data[i] * this->Data[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
T vtkQuaternion<T>::Norm() const
|
|
{
|
|
return sqrt(this->SquaredNorm());
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::ToIdentity()
|
|
{
|
|
this->Set(1.0, 0.0, 0.0, 0.0);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::Identity()
|
|
{
|
|
vtkQuaternion<T> identity(1.0, 0.0, 0.0, 0.0);
|
|
return identity;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
T vtkQuaternion<T>::Normalize()
|
|
{
|
|
T norm = this->Norm();
|
|
if (norm != 0.0)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] /= norm;
|
|
}
|
|
}
|
|
return norm;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::Normalized() const
|
|
{
|
|
vtkQuaternion<T> temp(*this);
|
|
temp.Normalize();
|
|
return temp;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::Conjugate()
|
|
{
|
|
for (int i = 1; i < 4; ++i)
|
|
{
|
|
this->Data[i] *= -1.0;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::Conjugated() const
|
|
{
|
|
vtkQuaternion<T> ret(*this);
|
|
ret.Conjugate();
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::Invert()
|
|
{
|
|
T squareNorm = this->SquaredNorm();
|
|
if (squareNorm != 0.0)
|
|
{
|
|
this->Conjugate();
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] /= squareNorm;
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::Inverse() const
|
|
{
|
|
vtkQuaternion<T> ret(*this);
|
|
ret.Invert();
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
template <typename CastTo>
|
|
vtkQuaternion<CastTo> vtkQuaternion<T>::Cast() const
|
|
{
|
|
vtkQuaternion<CastTo> result;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
result[i] = static_cast<CastTo>(this->Data[i]);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::Set(const T& w, const T& x, const T& y, const T& z)
|
|
{
|
|
this->Data[0] = w;
|
|
this->Data[1] = x;
|
|
this->Data[2] = y;
|
|
this->Data[3] = z;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::Set(T quat[4])
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] = quat[i];
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::Get(T quat[4]) const
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
quat[i] = this->Data[i];
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetW(const T& w)
|
|
{
|
|
this->Data[0] = w;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
const T& vtkQuaternion<T>::GetW() const
|
|
{
|
|
return this->Data[0];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetX(const T& x)
|
|
{
|
|
this->Data[1] = x;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
const T& vtkQuaternion<T>::GetX() const
|
|
{
|
|
return this->Data[1];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetY(const T& y)
|
|
{
|
|
this->Data[2] = y;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
const T& vtkQuaternion<T>::GetY() const
|
|
{
|
|
return this->Data[2];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetZ(const T& z)
|
|
{
|
|
this->Data[3] = z;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
const T& vtkQuaternion<T>::GetZ() const
|
|
{
|
|
return this->Data[3];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
T vtkQuaternion<T>::GetRotationAngleAndAxis(T axis[3]) const
|
|
{
|
|
T w = this->GetW();
|
|
T x = this->GetX();
|
|
T y = this->GetY();
|
|
T z = this->GetZ();
|
|
T f = sqrt(x * x + y * y + z * z);
|
|
if (f != 0.0)
|
|
{
|
|
axis[0] = x / f;
|
|
axis[1] = y / f;
|
|
axis[2] = z / f;
|
|
}
|
|
else
|
|
{
|
|
w = 1.0;
|
|
axis[0] = 0.0;
|
|
axis[1] = 0.0;
|
|
axis[2] = 0.0;
|
|
}
|
|
|
|
// atan2() provides a more accurate angle result than acos()
|
|
return 2.0 * atan2(f, w);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetRotationAngleAndAxis(T angle, T axis[3])
|
|
{
|
|
this->SetRotationAngleAndAxis(angle, axis[0], axis[1], axis[2]);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::SetRotationAngleAndAxis(const T& angle, const T& x, const T& y, const T& z)
|
|
{
|
|
T axisNorm = x * x + y * y + z * z;
|
|
if (axisNorm != 0.0)
|
|
{
|
|
T f = sin(0.5 * angle);
|
|
this->SetW(cos(0.5 * angle));
|
|
this->SetX((x / axisNorm) * f);
|
|
this->SetY((y / axisNorm) * f);
|
|
this->SetZ((z / axisNorm) * f);
|
|
}
|
|
else
|
|
{
|
|
// set the quaternion for "no rotation"
|
|
this->Set(1.0, 0.0, 0.0, 0.0);
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator+(const vtkQuaternion<T>& q) const
|
|
{
|
|
vtkQuaternion<T> ret;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
ret[i] = this->Data[i] + q[i];
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator-(const vtkQuaternion<T>& q) const
|
|
{
|
|
vtkQuaternion<T> ret;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
ret[i] = this->Data[i] - q[i];
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator*(const vtkQuaternion<T>& q) const
|
|
{
|
|
vtkQuaternion<T> ret;
|
|
T ww = this->Data[0] * q[0];
|
|
T wx = this->Data[0] * q[1];
|
|
T wy = this->Data[0] * q[2];
|
|
T wz = this->Data[0] * q[3];
|
|
|
|
T xw = this->Data[1] * q[0];
|
|
T xx = this->Data[1] * q[1];
|
|
T xy = this->Data[1] * q[2];
|
|
T xz = this->Data[1] * q[3];
|
|
|
|
T yw = this->Data[2] * q[0];
|
|
T yx = this->Data[2] * q[1];
|
|
T yy = this->Data[2] * q[2];
|
|
T yz = this->Data[2] * q[3];
|
|
|
|
T zw = this->Data[3] * q[0];
|
|
T zx = this->Data[3] * q[1];
|
|
T zy = this->Data[3] * q[2];
|
|
T zz = this->Data[3] * q[3];
|
|
|
|
ret[0] = ww - xx - yy - zz;
|
|
ret[1] = wx + xw + yz - zy;
|
|
ret[2] = wy - xz + yw + zx;
|
|
ret[3] = wz + xy - yx + zw;
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator*(const T& scalar) const
|
|
{
|
|
vtkQuaternion<T> ret;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
ret[i] = this->Data[i] * scalar;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::operator*=(const T& scalar) const
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] *= scalar;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator/(const vtkQuaternion<T>& q) const
|
|
{
|
|
vtkQuaternion<T> inverseQuaternion = q.Inverse();
|
|
return (*this) * inverseQuaternion;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::operator/(const T& scalar) const
|
|
{
|
|
vtkQuaternion<T> ret;
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
ret[i] = this->Data[i] / scalar;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::operator/=(const T& scalar)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] /= scalar;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::ToMatrix3x3(T A[3][3]) const
|
|
{
|
|
T ww = this->Data[0] * this->Data[0];
|
|
T wx = this->Data[0] * this->Data[1];
|
|
T wy = this->Data[0] * this->Data[2];
|
|
T wz = this->Data[0] * this->Data[3];
|
|
|
|
T xx = this->Data[1] * this->Data[1];
|
|
T yy = this->Data[2] * this->Data[2];
|
|
T zz = this->Data[3] * this->Data[3];
|
|
|
|
T xy = this->Data[1] * this->Data[2];
|
|
T xz = this->Data[1] * this->Data[3];
|
|
T yz = this->Data[2] * this->Data[3];
|
|
|
|
T rr = xx + yy + zz;
|
|
// normalization factor, just in case quaternion was not normalized
|
|
T f;
|
|
if (ww + rr == 0.0) // means the quaternion is (0, 0, 0, 0)
|
|
{
|
|
A[0][0] = 0.0;
|
|
A[1][0] = 0.0;
|
|
A[2][0] = 0.0;
|
|
A[0][1] = 0.0;
|
|
A[1][1] = 0.0;
|
|
A[2][1] = 0.0;
|
|
A[0][2] = 0.0;
|
|
A[1][2] = 0.0;
|
|
A[2][2] = 0.0;
|
|
return;
|
|
}
|
|
f = 1.0 / (ww + rr);
|
|
|
|
T s = (ww - rr) * f;
|
|
f *= 2.0;
|
|
|
|
A[0][0] = xx * f + s;
|
|
A[1][0] = (xy + wz) * f;
|
|
A[2][0] = (xz - wy) * f;
|
|
|
|
A[0][1] = (xy - wz) * f;
|
|
A[1][1] = yy * f + s;
|
|
A[2][1] = (yz + wx) * f;
|
|
|
|
A[0][2] = (xz + wy) * f;
|
|
A[1][2] = (yz - wx) * f;
|
|
A[2][2] = zz * f + s;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// The solution is based on
|
|
// Berthold K. P. Horn (1987),
|
|
// "Closed-form solution of absolute orientation using unit quaternions,"
|
|
// Journal of the Optical Society of America A, 4:629-642
|
|
template <typename T>
|
|
void vtkQuaternion<T>::FromMatrix3x3(const T A[3][3])
|
|
{
|
|
T n[4][4];
|
|
|
|
// on-diagonal elements
|
|
n[0][0] = A[0][0] + A[1][1] + A[2][2];
|
|
n[1][1] = A[0][0] - A[1][1] - A[2][2];
|
|
n[2][2] = -A[0][0] + A[1][1] - A[2][2];
|
|
n[3][3] = -A[0][0] - A[1][1] + A[2][2];
|
|
|
|
// off-diagonal elements
|
|
n[0][1] = n[1][0] = A[2][1] - A[1][2];
|
|
n[0][2] = n[2][0] = A[0][2] - A[2][0];
|
|
n[0][3] = n[3][0] = A[1][0] - A[0][1];
|
|
|
|
n[1][2] = n[2][1] = A[1][0] + A[0][1];
|
|
n[1][3] = n[3][1] = A[0][2] + A[2][0];
|
|
n[2][3] = n[3][2] = A[2][1] + A[1][2];
|
|
|
|
T eigenvectors[4][4];
|
|
T eigenvalues[4];
|
|
|
|
// convert into format that JacobiN can use,
|
|
// then use Jacobi to find eigenvalues and eigenvectors
|
|
T* nTemp[4];
|
|
T* eigenvectorsTemp[4];
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
nTemp[i] = n[i];
|
|
eigenvectorsTemp[i] = eigenvectors[i];
|
|
}
|
|
vtkMath::JacobiN(nTemp, 4, eigenvalues, eigenvectorsTemp);
|
|
|
|
// the first eigenvector is the one we want
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
this->Data[i] = eigenvectors[i][0];
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// This returns the constant angular velocity interpolation of two quaternions
|
|
// on the unit quaternion sphere :
|
|
// http://web.mit.edu/2.998/www/QuaternionReport1.pdf
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::Slerp(T t, const vtkQuaternion<T>& q1) const
|
|
{
|
|
// Canonical scalar product on quaternion
|
|
T cosTheta = this->GetW() * q1.GetW() + this->GetX() * q1.GetX() + this->GetY() * q1.GetY() +
|
|
this->GetZ() * q1.GetZ();
|
|
|
|
// To prevent the SLERP interpolation from taking the long path
|
|
// we first check the relative orientation of the two quaternions
|
|
// If the angle is superior to 90 degrees we take the opposite quaternion
|
|
// which is closer and represents the same rotation
|
|
vtkQuaternion<T> qClosest = q1;
|
|
if (cosTheta < 0)
|
|
{
|
|
cosTheta = -cosTheta;
|
|
qClosest = qClosest * -1;
|
|
}
|
|
|
|
// To avoid division by zero, perform a linear interpolation (LERP), if our
|
|
// quarternions are nearly in the same direction, otherwise resort
|
|
// to spherical linear interpolation. In the limiting case (for small
|
|
// angles), SLERP is equivalent to LERP.
|
|
T t1, t2;
|
|
if ((1.0 - fabs(cosTheta)) < 1e-6)
|
|
{
|
|
t1 = 1.0 - t;
|
|
t2 = t;
|
|
}
|
|
else
|
|
{
|
|
// Angle (defined by the canonical scalar product for quaternions)
|
|
// between the two quaternions
|
|
const T theta = acos(cosTheta);
|
|
t1 = sin((1.0 - t) * theta) / sin(theta);
|
|
t2 = sin(t * theta) / sin(theta);
|
|
}
|
|
|
|
return (*this) * t1 + qClosest * t2;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::InnerPoint(
|
|
const vtkQuaternion<T>& q1, const vtkQuaternion<T>& q2) const
|
|
{
|
|
vtkQuaternion<T> qInv = q1.Inverse();
|
|
vtkQuaternion<T> qL = qInv * q2;
|
|
vtkQuaternion<T> qR = qInv * (*this);
|
|
|
|
vtkQuaternion<T> qLLog = qL.UnitLog();
|
|
vtkQuaternion<T> qRLog = qR.UnitLog();
|
|
vtkQuaternion<T> qSum = qLLog + qRLog;
|
|
T w = qSum.GetW();
|
|
qSum /= -4.0;
|
|
qSum.SetW(w);
|
|
|
|
vtkQuaternion<T> qExp = qSum.UnitExp();
|
|
return q1 * qExp;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::ToUnitLog()
|
|
{
|
|
T axis[3];
|
|
T angle = 0.5 * this->GetRotationAngleAndAxis(axis);
|
|
|
|
this->Set(0.0, angle * axis[0], angle * axis[1], angle * axis[2]);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::UnitLog() const
|
|
{
|
|
vtkQuaternion<T> unitLog(*this);
|
|
unitLog.ToUnitLog();
|
|
return unitLog;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::ToUnitExp()
|
|
{
|
|
T x = this->GetX();
|
|
T y = this->GetY();
|
|
T z = this->GetZ();
|
|
T angle = sqrt(x * x + y * y + z * z);
|
|
T sinAngle = sin(angle);
|
|
T cosAngle = cos(angle);
|
|
if (angle != 0.0)
|
|
{
|
|
x /= angle;
|
|
y /= angle;
|
|
z /= angle;
|
|
}
|
|
|
|
this->Set(cosAngle, sinAngle * x, sinAngle * y, sinAngle * z);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::UnitExp() const
|
|
{
|
|
vtkQuaternion<T> unitExp(*this);
|
|
unitExp.ToUnitExp();
|
|
return unitExp;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
void vtkQuaternion<T>::NormalizeWithAngleInDegrees()
|
|
{
|
|
this->Normalize();
|
|
this->SetW(vtkMath::DegreesFromRadians(this->GetW()));
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
template <typename T>
|
|
vtkQuaternion<T> vtkQuaternion<T>::NormalizedWithAngleInDegrees() const
|
|
{
|
|
vtkQuaternion<T> unitVTK(*this);
|
|
unitVTK.Normalize();
|
|
unitVTK.SetW(vtkMath::DegreesFromRadians(unitVTK.GetW()));
|
|
return unitVTK;
|
|
}
|
|
|
|
#endif
|