mirror of https://github.com/CGAL/cgal
438 lines
10 KiB
C++
438 lines
10 KiB
C++
//********************************************
|
|
// Quaternion.cpp
|
|
//********************************************
|
|
// class CQuaternion
|
|
//********************************************
|
|
// mmeyer@gg.caltech.edu
|
|
// Created : 09/07/00
|
|
// Modified : 09/07/00
|
|
//********************************************
|
|
|
|
#include <math.h>
|
|
#include "Quaternion.h"
|
|
|
|
#ifdef _DEBUG
|
|
#define new DEBUG_NEW
|
|
#undef THIS_FILE
|
|
static char THIS_FILE[] = __FILE__;
|
|
#endif
|
|
|
|
//////////////////////////////////////////////
|
|
// CONSTRUCTORS
|
|
//////////////////////////////////////////////
|
|
|
|
//********************************************
|
|
// Constructor
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const float s, const float x, const float y, const float z)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(x,y,z);
|
|
}
|
|
|
|
//********************************************
|
|
// Constructor
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const CQuaternion &quat)
|
|
{
|
|
Set(quat);
|
|
}
|
|
|
|
//********************************************
|
|
// Constructor
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const CQuaternion *pQuat)
|
|
{
|
|
Set(pQuat);
|
|
}
|
|
|
|
//********************************************
|
|
// Copy
|
|
//********************************************
|
|
void CQuaternion::Copy(const CQuaternion &quat)
|
|
{
|
|
Set(quat);
|
|
}
|
|
|
|
//********************************************
|
|
// Copy
|
|
//********************************************
|
|
void CQuaternion::Copy(const CQuaternion *pQuat)
|
|
{
|
|
Set(pQuat);
|
|
}
|
|
|
|
//********************************************
|
|
// Constructor
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const float s, const CVector3d *pVector)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(pVector);
|
|
}
|
|
|
|
//********************************************
|
|
// Constructor
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const float s, const CVector3d& vector)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(vector);
|
|
}
|
|
|
|
//********************************************
|
|
// Constructor
|
|
// This assumes unit vecFrom and vecTo
|
|
//********************************************
|
|
CQuaternion::CQuaternion(const CVector3d &vecFrom, const CVector3d &vecTo)
|
|
{
|
|
CVector3d vecHalf = vecTo + vecFrom;
|
|
vecHalf.Normalize();
|
|
m_s = vecHalf.Dot(vecTo);
|
|
m_v = vecHalf.Cross(vecTo);
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////
|
|
// DATA
|
|
//////////////////////////////////////////////
|
|
|
|
|
|
//********************************************
|
|
// Clear
|
|
// Clears to a unit quaternion (no rotation)
|
|
//********************************************
|
|
void
|
|
CQuaternion::Clear()
|
|
{
|
|
Set(1.0f,0.0f,0.0f,0.0f);
|
|
}
|
|
|
|
//********************************************
|
|
// Set
|
|
//********************************************
|
|
void
|
|
CQuaternion::Set(const float s, const float x, const float y, const float z)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(x,y,z);
|
|
}
|
|
|
|
//********************************************
|
|
// Set
|
|
//********************************************
|
|
void
|
|
CQuaternion::Set(const float s, const CVector3d &v)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(v);
|
|
}
|
|
|
|
//********************************************
|
|
// Set
|
|
//********************************************
|
|
void
|
|
CQuaternion::Set(const float s, const CVector3d *pV)
|
|
{
|
|
m_s = s;
|
|
m_v.Set(pV);
|
|
}
|
|
|
|
//********************************************
|
|
// Set
|
|
//********************************************
|
|
void
|
|
CQuaternion::Set(const CQuaternion *pQuat)
|
|
{
|
|
Set(pQuat->s(),pQuat->v());
|
|
}
|
|
|
|
//********************************************
|
|
// Set
|
|
//********************************************
|
|
void
|
|
CQuaternion::Set(const CQuaternion &quat)
|
|
{
|
|
Set(quat.s(),quat.v());
|
|
}
|
|
|
|
//********************************************
|
|
// SetRotation
|
|
//********************************************
|
|
void
|
|
CQuaternion::SetRotation(float ax, float ay, float az, float radAngle)
|
|
{
|
|
float halfAngle = radAngle / 2.f;
|
|
m_s = float(cos(halfAngle));
|
|
m_v.Set(ax,ay,az);
|
|
m_v *= float(sin(halfAngle));
|
|
}
|
|
|
|
//********************************************
|
|
// GetMatrix
|
|
// Construct rotation matrix from (possibly non-unit) quaternion.
|
|
// Assumes matrix is used to multiply column vector on the left:
|
|
// Vnew = Matrix * Vold. Works correctly for right-handed
|
|
// coordinate systems and right-handed rotations.
|
|
//********************************************
|
|
void
|
|
CQuaternion::GetMatrix(float *mat) const
|
|
{
|
|
double Nq = LengthSquared();
|
|
double c = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
|
|
double xc = x()*c, yc = y()*c, zc = z()*c;
|
|
double sx = s()*xc, sy = s()*yc, sz = s()*zc;
|
|
double xx = x()*xc, xy = x()*yc, xz = x()*zc;
|
|
double yy = y()*yc, yz = y()*zc, zz = z()*zc;
|
|
mat[0] = float(1.0 - (yy + zz));
|
|
mat[1] = float(xy - sz);
|
|
mat[2] = float(xz + sy);
|
|
mat[4] = float(xy + sz);
|
|
mat[5] = float(1.0 - (xx + zz));
|
|
mat[6] = float(yz - sx);
|
|
mat[8] = float(xz - sy);
|
|
mat[9] = float(yz + sx);
|
|
mat[10] = float(1.0 - (xx + yy));
|
|
mat[3] = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0.0f;
|
|
mat[15] = 1.0f;
|
|
}
|
|
|
|
//********************************************
|
|
// GetMatrix
|
|
// Construct rotation matrix from (possibly non-unit) quaternion.
|
|
// Assumes matrix is used to multiply column vector on the left:
|
|
// Vnew = Matrix * Vold. Works correctly for right-handed
|
|
// coordinate systems and right-handed rotations.
|
|
//********************************************
|
|
CMatrix44
|
|
CQuaternion::GetMatrix() const
|
|
{
|
|
CMatrix44 mat;
|
|
double Nq = LengthSquared();
|
|
double c = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
|
|
double xc = x()*c, yc = y()*c, zc = z()*c;
|
|
double sx = s()*xc, sy = s()*yc, sz = s()*zc;
|
|
double xx = x()*xc, xy = x()*yc, xz = x()*zc;
|
|
double yy = y()*yc, yz = y()*zc, zz = z()*zc;
|
|
mat[0][0] = float(1.0 - (yy + zz));
|
|
mat[0][1] = float(xy - sz);
|
|
mat[0][2] = float(xz + sy);
|
|
mat[1][0] = float(xy + sz);
|
|
mat[1][1] = float(1.0 - (xx + zz));
|
|
mat[1][2] = float(yz - sx);
|
|
mat[2][0] = float(xz - sy);
|
|
mat[2][1] = float(yz + sx);
|
|
mat[2][2] = float(1.0 - (xx + yy));
|
|
mat[0][3] = mat[1][3] = mat[2][3] = mat[3][0] = mat[3][1] = mat[3][2] = 0.0f;
|
|
mat[3][3] = 1.0f;
|
|
return mat;
|
|
}
|
|
|
|
|
|
//////////////////////////////////////////////
|
|
// OPERATORS
|
|
//////////////////////////////////////////////
|
|
|
|
//********************************************
|
|
// Operator +=
|
|
//********************************************
|
|
CQuaternion&
|
|
CQuaternion::operator+=(const CQuaternion& rQuad)
|
|
{
|
|
m_s += rQuad.s();
|
|
m_v += rQuad.v();
|
|
return *this;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator +=
|
|
//********************************************
|
|
CQuaternion&
|
|
CQuaternion::operator+=(const CQuaternion* pQuad)
|
|
{
|
|
m_s += pQuad->s();
|
|
m_v += pQuad->m_v;
|
|
return *this;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator -=
|
|
//********************************************
|
|
CQuaternion&
|
|
CQuaternion::operator-=(const CQuaternion& rQuad)
|
|
{
|
|
m_s -= rQuad.s();
|
|
m_v -= rQuad.v();
|
|
return *this;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator -=
|
|
//********************************************
|
|
CQuaternion&
|
|
CQuaternion::operator-=(const CQuaternion* pQuad)
|
|
{
|
|
m_s -= pQuad->s();
|
|
m_v -= pQuad->v();
|
|
return *this;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator *=
|
|
//********************************************
|
|
CQuaternion&
|
|
CQuaternion::operator*=(const float d)
|
|
{
|
|
m_s *= d;
|
|
m_v *= d;
|
|
return *this;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator -
|
|
// Nondestructive unary -
|
|
// Returns a new vector.
|
|
//********************************************
|
|
CQuaternion
|
|
CQuaternion::operator -() const
|
|
{
|
|
return CQuaternion(-m_s,-m_v);
|
|
}
|
|
|
|
//********************************************
|
|
// Operator +
|
|
//********************************************
|
|
CQuaternion
|
|
operator+(const CQuaternion& u, const CQuaternion& v)
|
|
{
|
|
return CQuaternion(u.m_s+v.m_s,u.m_v+v.m_v);
|
|
}
|
|
|
|
//********************************************
|
|
// Operator -
|
|
//********************************************
|
|
CQuaternion
|
|
operator-(const CQuaternion& u, const CQuaternion& v)
|
|
{
|
|
return CQuaternion(u.m_s-v.m_s,u.m_v-v.m_v);
|
|
}
|
|
|
|
//********************************************
|
|
// Operator *
|
|
//********************************************
|
|
CQuaternion
|
|
operator*(const CQuaternion& u, const CQuaternion& v)
|
|
{
|
|
CQuaternion w;
|
|
CVector3d a = u.m_v;
|
|
CVector3d b = v.m_v;
|
|
float ws = u.m_s * v.m_s - a.Dot(b);
|
|
CVector3d wv = u.m_s*b + v.m_s*a + a.Cross(b);
|
|
w.Set(ws,wv);
|
|
return w;
|
|
}
|
|
|
|
//********************************************
|
|
// Operator *
|
|
//********************************************
|
|
CQuaternion
|
|
operator*(const float s, const CQuaternion& u)
|
|
{
|
|
return CQuaternion(u.m_s * s, u.m_v * s);
|
|
}
|
|
|
|
//********************************************
|
|
// Operator ==
|
|
//********************************************
|
|
int
|
|
operator==(const CQuaternion& q1, const CQuaternion& q2)
|
|
{
|
|
return (q1.m_s == q2.m_s &&
|
|
|
|
q1.m_v == q2.m_v);
|
|
}
|
|
|
|
//********************************************
|
|
// Equals
|
|
// Determines if two quaternions are equal
|
|
// within a tolerence (squared distance).
|
|
//********************************************
|
|
int
|
|
CQuaternion::Equals(const CQuaternion& q, const float tolerence) const
|
|
{
|
|
CQuaternion diff = *this - q;
|
|
|
|
return diff.LengthSquared() <= tolerence;
|
|
}
|
|
|
|
//////////////////////////////////////////////
|
|
//////////////////////////////////////////////
|
|
// PROCESSING
|
|
//////////////////////////////////////////////
|
|
//////////////////////////////////////////////
|
|
|
|
//********************************************
|
|
// Normalize
|
|
//********************************************
|
|
double
|
|
CQuaternion::Normalize()
|
|
{
|
|
double len = Length();
|
|
if(len != 0.0f)
|
|
(*this) *= float(1.0/len);
|
|
else
|
|
Set(0.f,0.f,0.f,0.f);
|
|
|
|
return len;
|
|
}
|
|
|
|
//********************************************
|
|
// Length
|
|
//********************************************
|
|
double
|
|
CQuaternion::Length()const
|
|
{
|
|
return sqrt((double)m_s *(double)m_s +
|
|
(double)m_v.x()*(double)m_v.x() +
|
|
(double)m_v.y()*(double)m_v.y() +
|
|
(double)m_v.z()*(double)m_v.z());
|
|
}
|
|
|
|
//********************************************
|
|
// LengthSquared
|
|
//********************************************
|
|
double
|
|
CQuaternion::LengthSquared()const
|
|
{
|
|
return ((double)m_s *(double)m_s +
|
|
(double)m_v.x()*(double)m_v.x() +
|
|
(double)m_v.y()*(double)m_v.y() +
|
|
(double)m_v.z()*(double)m_v.z());
|
|
}
|
|
|
|
//********************************************
|
|
// Negate
|
|
// Negate each component of the quaternion
|
|
//********************************************
|
|
void
|
|
CQuaternion::Negate()
|
|
{
|
|
m_s = -m_s;
|
|
m_v = -m_v;
|
|
}
|
|
|
|
//********************************************
|
|
// Conjugate
|
|
// Return the conjugate of the quaternion
|
|
//********************************************
|
|
CQuaternion
|
|
CQuaternion::Conjugate()
|
|
{
|
|
return CQuaternion(m_s,-m_v[0],-m_v[1],-m_v[2]);
|
|
}
|
|
|
|
// ** EOF **
|
|
|
|
|
|
|