Skip to content

Commit

Permalink
xrCore/_quaternion.h: cleanup code
Browse files Browse the repository at this point in the history
  • Loading branch information
@Xottab-DUTY
Xottab-DUTY committed Aug 8, 2017
1 parent b24059e commit 1f3d298
Showing 1 changed file with 50 additions and 44 deletions.
94 changes: 50 additions & 44 deletions src/xrCore/_quaternion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,11 @@
#include "_vector3d.h"

/***************************************************************************
The quatern module contains basic support for a quaternion object.
The quaternion module contains basic support for a quaternion object.
quaternions are an extension of complex numbers that allows an
expression for rotation that can be easily interpolated. Quaternions are also
more numericaly stable for repeated rotations than matrices.
more numerically stable for repeated rotations than matrices.
A quaternion is a 4 element 'vector3' [w,x,y,z] where:
Expand Down Expand Up @@ -88,7 +88,7 @@
(which is a unit vector3, of course). A unit quaternion can also be
thought of as a point on the surface of a four-dimensional hypersphere,
so if you try to interpolate between two unit quaternions, you can get
an intermediate rotation. Gamasutra describes Shoemake's spherical
an intermediate rotation. Gamasutra describes Shoemake's spherical
linear interpolation method, but I think getting the logarithm of the
quaternions and performing linear interpolation is easier. The
logarithm of a quaternion is given by
Expand Down Expand Up @@ -147,14 +147,13 @@ struct _matrix;
template <class T>
struct _quaternion
{
public:
typedef T TYPE;
typedef _quaternion<T> Self;
typedef Self& SelfRef;
typedef const Self& SelfCRef;
using TYPE = T;
using Self = _quaternion<T>;
using SelfRef = Self&;
using SelfCRef = const Self&;

private:
IC T _asin_(T val)
static T _asin_(T val)
{
const T c1 = 0.892399f;
const T c3 = 1.693204f;
Expand All @@ -166,19 +165,22 @@ struct _quaternion

return d;
}
IC T _acos_(T val) { return PI_DIV_2 - _asin_(val); }

static T _acos_(T val) { return PI_DIV_2 - _asin_(val); }

public:
T x, y, z, w;

IC SelfRef set(T W, T X, T Y, T Z) // don't normalize
SelfRef set(T W, T X, T Y, T Z) // don't normalize
{
x = X;
y = Y;
z = Z;
w = W;
return *this;
}
IC SelfRef set(SelfCRef Q) // don't normalize

SelfRef set(SelfCRef Q) // don't normalize
{
set(Q.w, Q.x, Q.y, Q.z);
return *this;
Expand All @@ -196,7 +198,7 @@ struct _quaternion
(w1*y2 - x1*z2 + y1*w2 + z1*x2)j {y3}
(w1*z2 + x1*y2 - y1*x2 + z1*w2)k {z3}
*/
IC SelfRef mul(SelfCRef q1l, SelfCRef q2l)
SelfRef mul(SelfCRef q1l, SelfCRef q2l)
{
VERIFY(q1l.isValid());
VERIFY(q2l.isValid());
Expand All @@ -211,15 +213,16 @@ struct _quaternion
return *this;
}

IC SelfRef add(SelfCRef q1, SelfCRef q2)
SelfRef add(SelfCRef q1, SelfCRef q2)
{
x = q1.x + q2.x;
y = q1.y + q2.y;
z = q1.z + q2.z;
w = q1.w + q2.w;
return *this;
}
IC SelfRef sub(SelfCRef q1, SelfCRef q2)

SelfRef sub(SelfCRef q1, SelfCRef q2)
{
x = q1.x - q2.x;
y = q1.y - q2.y;
Expand All @@ -228,15 +231,16 @@ struct _quaternion
return *this;
}

IC SelfRef add(SelfCRef q)
SelfRef add(SelfCRef q)
{
x += q.x;
y += q.y;
z += q.z;
w += q.w;
return *this;
}
IC SelfRef sub(SelfCRef q)

SelfRef sub(SelfCRef q)
{
x -= q.x;
y -= q.y;
Expand All @@ -246,7 +250,7 @@ struct _quaternion
}

// validates numerical stability
IC const bool isValid(void) const
const bool isValid() const
{
if ((w * w) < 0.0f)
return false;
Expand All @@ -260,7 +264,7 @@ struct _quaternion
}

// checks for Unit-length quanternion
IC const bool isUnit(void)
const bool isUnit()
{
T m = magnitude();

Expand All @@ -270,7 +274,7 @@ struct _quaternion
}

// normalizes Q to be a unit geQuaternion
IC SelfRef normalize(void)
SelfRef normalize()
{
T m, one_over_magnitude;

Expand All @@ -289,16 +293,19 @@ struct _quaternion
}

// inversion
IC SelfRef inverse(SelfCRef Q) { return set(Q.w, -Q.x, -Q.y, -Q.z); }
IC SelfRef inverse() { return set(w, -x, -y, -z); }
IC SelfRef inverse_with_w(SelfCRef Q) { return set(-Q.w, -Q.x, -Q.y, -Q.z); }
IC SelfRef inverse_with_w() { return set(-w, -x, -y, -z); }
SelfRef inverse(SelfCRef Q) { return set(Q.w, -Q.x, -Q.y, -Q.z); }
SelfRef inverse() { return set(w, -x, -y, -z); }
SelfRef inverse_with_w(SelfCRef Q) { return set(-Q.w, -Q.x, -Q.y, -Q.z); }
SelfRef inverse_with_w() { return set(-w, -x, -y, -z); }

// identity - no rotation
IC SelfRef identity(void) { return set(1.f, 0.f, 0.f, 0.f); }
SelfRef identity() { return set(1.f, 0.f, 0.f, 0.f); }

// square length
IC T magnitude(void) { return w * w + x * x + y * y + z * z; }
T magnitude() { return w * w + x * x + y * y + z * z; }

// makes unit rotation
IC SelfRef rotationYawPitchRoll(T _x, T _y, T _z)
SelfRef rotationYawPitchRoll(T _x, T _y, T _z)
{
T fSinYaw = _sin(_x * .5f);
T fCosYaw = _cos(_x * .5f);
Expand All @@ -315,9 +322,10 @@ struct _quaternion
}

// makes unit rotation
IC SelfRef rotationYawPitchRoll(const Fvector& ypr) { return rotationYawPitchRoll(ypr.x, ypr.y, ypr.z); }
SelfRef rotationYawPitchRoll(const Fvector& ypr) { return rotationYawPitchRoll(ypr.x, ypr.y, ypr.z); }

// set a quaternion from an axis and a rotation around the axis
IC SelfRef rotation(Fvector& axis, T angle)
SelfRef rotation(Fvector& axis, T angle)
{
T sinTheta;

Expand All @@ -333,7 +341,7 @@ struct _quaternion
// returns TRUE if there is an axis.
// returns FALSE if there is no axis (and Axis is set to 0,0,0, and Theta is 0)

IC BOOL get_axis_angle(Fvector& axis, T& angle)
bool get_axis_angle(Fvector& axis, T& angle)
{
T s = _sqrt(x * x + y * y + z * z);
if (s > EPS_S)
Expand All @@ -345,12 +353,9 @@ struct _quaternion
angle = 2.0f * atan2(s, w);
return true;
}
else
{
axis.x = axis.y = axis.z = 0.0f;
angle = 0.0f;
return false;
}
axis.x = axis.y = axis.z = 0.0f;
angle = 0.0f;
return false;
}

// spherical interpolation between q0 and q1. 0<=t<=1
Expand Down Expand Up @@ -403,19 +408,19 @@ struct _quaternion
return *this;
}

// return TRUE if quaternions differ elementwise by less than Tolerance.
IC BOOL cmp(SelfCRef Q, T Tolerance = 0.0001f)
// return true if quaternions differ elementwise by less than Tolerance.
bool cmp(SelfCRef Q, T Tolerance = 0.0001f)
{
if ( // they are the same but with opposite signs
if (// they are the same but with opposite signs
((_abs(x + Q.x) <= Tolerance) && (_abs(y + Q.y) <= Tolerance) && (_abs(z + Q.z) <= Tolerance) &&
(_abs(w + Q.w) <= Tolerance)) || // they are the same with same signs
((_abs(x - Q.x) <= Tolerance) && (_abs(y - Q.y) <= Tolerance) && (_abs(z - Q.z) <= Tolerance) &&
(_abs(w - Q.w) <= Tolerance)))
return true;
else
return false;
return false;
}
IC SelfRef ln(SelfCRef Q)

SelfRef ln(SelfCRef Q)
{
T n = Q.x * Q.x + Q.y * Q.y + Q.z * Q.z;
T r = _sqrt(n);
Expand All @@ -426,7 +431,8 @@ struct _quaternion
w = .5f * _log(n + Q.w * Q.w);
return *this;
}
IC SelfRef exp(SelfCRef Q)

SelfRef exp(SelfCRef Q)
{
T r = _sqrt(Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
T et = expf(Q.w);
Expand All @@ -443,7 +449,7 @@ typedef _quaternion<float> Fquaternion;
typedef _quaternion<double> Dquaternion;

template <class T>
BOOL _valid(const _quaternion<T>& s)
bool _valid(const _quaternion<T>& s)
{
return _valid(s.x) && _valid(s.y) && _valid(s.z) && _valid(s.w);
}
Expand Down

0 comments on commit 1f3d298

Please sign in to comment.