Adjusted workflow.
This commit is contained in:
396
include/ehs/Quat.h
Normal file
396
include/ehs/Quat.h
Normal file
@@ -0,0 +1,396 @@
|
||||
#pragma once
|
||||
|
||||
#include "EHS.h"
|
||||
#include "Math.h"
|
||||
#include "Mat4.h"
|
||||
#include "Vec3.h"
|
||||
|
||||
namespace ehs
|
||||
{
|
||||
template<typename T>
|
||||
class Quat
|
||||
{
|
||||
public:
|
||||
T w;
|
||||
T x;
|
||||
T y;
|
||||
T z;
|
||||
|
||||
Quat()
|
||||
: w(0), x(0), y(0), z(0)
|
||||
{
|
||||
}
|
||||
|
||||
Quat(const T w, const T x, const T y, const T z)
|
||||
: w(w), x(x), y(y), z(z)
|
||||
{
|
||||
}
|
||||
|
||||
Quat(const T yaw, const T pitch, const T roll)
|
||||
: w(0), x(0), y(0), z(0)
|
||||
{
|
||||
T c1 = cos(yaw / 2);
|
||||
T c2 = cos(pitch / 2);
|
||||
T c3 = cos(roll / 2);
|
||||
T s1 = sin(yaw / 2);
|
||||
T s2 = sin(pitch / 2);
|
||||
T s3 = sin(roll / 2);
|
||||
|
||||
w = c1 * c2 * c3 - s1 * s2 * s3;
|
||||
x = s1 * s2 * c3 + c1 * c2 * s3;
|
||||
y = s1 * c2 * c3 + c1 * s2 * s3;
|
||||
z = c1 * s2 * c3 - s1 * c2 * s3;
|
||||
}
|
||||
|
||||
explicit Quat(const Vec3<T>& euler)
|
||||
: w(0), x(0), y(0), z(0)
|
||||
{
|
||||
T c1 = cos(euler.x / 2);
|
||||
T c2 = cos(euler.y / 2);
|
||||
T c3 = cos(euler.z / 2);
|
||||
T s1 = sin(euler.x / 2);
|
||||
T s2 = sin(euler.y / 2);
|
||||
T s3 = sin(euler.z / 2);
|
||||
|
||||
w = c1 * c2 * c3 - s1 * s2 * s3;
|
||||
x = s1 * s2 * c3 + c1 * c2 * s3;
|
||||
y = s1 * c2 * c3 + c1 * s2 * s3;
|
||||
z = c1 * s2 * c3 - s1 * c2 * s3;
|
||||
}
|
||||
|
||||
Quat(const Vec3<T>& n, const T a)
|
||||
: w(cosf(a / 2)), x(n.x * sinf(a / 2)), y(n.y * sinf(a / 2)), z(n.z * sinf(a / 2))
|
||||
{
|
||||
}
|
||||
|
||||
explicit Quat(const Mat4<T>& rotMatrix)
|
||||
: w(0), x(0), y(0), z(0)
|
||||
{
|
||||
ToQuaternion(rotMatrix);
|
||||
}
|
||||
|
||||
Quat(const Quat& quat)
|
||||
: w(quat.w), x(quat.x), y(quat.y), z(quat.z)
|
||||
{
|
||||
}
|
||||
|
||||
explicit Quat(const T scalar)
|
||||
: w(scalar), x(scalar), y(scalar), z(scalar)
|
||||
{
|
||||
}
|
||||
|
||||
Quat& operator=(const Quat& quat)
|
||||
{
|
||||
if (this == &quat)
|
||||
return *this;
|
||||
|
||||
w = quat.w;
|
||||
x = quat.x;
|
||||
y = quat.y;
|
||||
z = quat.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quat& operator=(const Mat4<T>& rotMatrix)
|
||||
{
|
||||
ToQuaternion(rotMatrix);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Quat operator+(const Quat& other) const
|
||||
{
|
||||
return {w + other.w, x + other.x, y + other.y, z + other.z};
|
||||
}
|
||||
|
||||
Quat operator-() const
|
||||
{
|
||||
return {-w, -x, -y, -z};
|
||||
}
|
||||
|
||||
Quat operator-(const Quat& other) const
|
||||
{
|
||||
return {w - other.w, x - other.x, y - other.y, z - other.z};
|
||||
}
|
||||
|
||||
Quat operator*(const T scalar)
|
||||
{
|
||||
return {w * scalar, x * scalar, x * scalar, x * scalar};
|
||||
}
|
||||
|
||||
Quat operator*(const Quat& other)
|
||||
{
|
||||
return Quat
|
||||
(
|
||||
w * other.w - x * other.x - y * other.y - z * other.z,
|
||||
w * other.x + x * other.w + y * other.z - z * other.y,
|
||||
w * other.y - x * other.z + y * other.w + z * other.x,
|
||||
w * other.z + x * other.y - y * other.x + z * other.w
|
||||
);
|
||||
}
|
||||
|
||||
Vec3<T> operator*(const Vec3<T>& vect)
|
||||
{
|
||||
Quat tmp(0, vect[0], vect[1], vect[2]);
|
||||
Vec3<T> tmpVect(x, y, z);
|
||||
|
||||
Vec3<T> vcV = tmpVect.CrossProduct(vect);
|
||||
return vect + vcV * (2 * w) + tmpVect.CrossProduct(vcV) * 2;
|
||||
}
|
||||
|
||||
Quat operator^(const T t)
|
||||
{
|
||||
Vec3<T> n;
|
||||
T a;
|
||||
|
||||
ToAxisAngle(&n, &a);
|
||||
|
||||
float at = a * t;
|
||||
|
||||
return Quat<T>(n, at);
|
||||
}
|
||||
|
||||
bool operator==(const Quat& quat) const
|
||||
{
|
||||
return w == quat.w && x == quat.x && y == quat.y && z == quat.z;
|
||||
}
|
||||
|
||||
bool operator!=(const Quat& quat) const
|
||||
{
|
||||
return w != quat.w || x != quat.x || y != quat.y || z == quat.z;
|
||||
}
|
||||
|
||||
T operator[](const UInt_64 index) const
|
||||
{
|
||||
switch (index)
|
||||
{
|
||||
case 0:
|
||||
return w;
|
||||
case 1:
|
||||
return x;
|
||||
case 2:
|
||||
return y;
|
||||
case 3:
|
||||
return z;
|
||||
default:
|
||||
return w;
|
||||
}
|
||||
}
|
||||
|
||||
T& operator[](const UInt_64 index)
|
||||
{
|
||||
switch (index)
|
||||
{
|
||||
case 0:
|
||||
return w;
|
||||
case 1:
|
||||
return x;
|
||||
case 2:
|
||||
return y;
|
||||
case 3:
|
||||
return z;
|
||||
default:
|
||||
return w;
|
||||
}
|
||||
}
|
||||
|
||||
void ToAxisAngle(Vec3<T>* vectAxis, T* flAngle)
|
||||
{
|
||||
Vec3<T> tmp(x, y, z);
|
||||
|
||||
if (tmp.GetDis2() < 0.0001f)
|
||||
*vectAxis = Vec3<T>(1, 0, 0);
|
||||
else
|
||||
*vectAxis = tmp.GetNorm();
|
||||
|
||||
*flAngle = acosf(w) * 2;
|
||||
*flAngle = Math::Degr<T>(*flAngle);
|
||||
}
|
||||
|
||||
void ToQuaternion(const Mat4<T>& rotMatrix)
|
||||
{
|
||||
T trace = rotMatrix[0][0] + rotMatrix[1][1] + rotMatrix[2][2];
|
||||
|
||||
if (trace > 0)
|
||||
{
|
||||
T s = 0.5f / Math::Sqrt<T>(trace + 1.0f);
|
||||
w = 0.25f / s;
|
||||
|
||||
x = (rotMatrix[2][1] - rotMatrix[1][2]) * s;
|
||||
y = (rotMatrix[0][2] - rotMatrix[2][0]) * s;
|
||||
z = (rotMatrix[1][0] - rotMatrix[0][1]) * s;
|
||||
} else
|
||||
{
|
||||
if ((rotMatrix[0][0] > rotMatrix[1][1]) && (rotMatrix[0][0] > rotMatrix[2][2]))
|
||||
{
|
||||
T s = 2.0f * Math::Sqrt(1.0f + rotMatrix[0][0] - rotMatrix[1][1] - rotMatrix[2][2]);
|
||||
w = (rotMatrix[2][1] - rotMatrix[1][2]) / s;
|
||||
x = 0.25f * s;
|
||||
y = (rotMatrix[0][1] + rotMatrix[1][0]) / s;
|
||||
z = (rotMatrix[0][2] + rotMatrix[2][0]) / s;
|
||||
} else if (rotMatrix[1][1] > rotMatrix[2][2])
|
||||
{
|
||||
T s = 2.0f * sqrtf(1.0f + rotMatrix[1][1] - rotMatrix[0][0] - rotMatrix[2][2]);
|
||||
w = (rotMatrix[0][2] - rotMatrix[2][0]) / s;
|
||||
x = (rotMatrix[0][1] + rotMatrix[1][0]) / s;
|
||||
y = 0.25f * s;
|
||||
z = (rotMatrix[1][2] + rotMatrix[2][1]) / s;
|
||||
} else
|
||||
{
|
||||
T s = 2.0f * sqrtf(1.0f + rotMatrix[2][2] - rotMatrix[0][0] - rotMatrix[1][1]);
|
||||
w = (rotMatrix[1][0] - rotMatrix[0][1]) / s;
|
||||
x = (rotMatrix[0][2] + rotMatrix[2][0]) / s;
|
||||
y = (rotMatrix[1][2] + rotMatrix[2][1]) / s;
|
||||
z = 0.25f * s;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Vec3<T> ToEulerAngle() const
|
||||
{
|
||||
Vec3<T> euler;
|
||||
|
||||
float ysqr = y * y;
|
||||
|
||||
float t0 = 2 * (w * x + y * z);
|
||||
float t1 = 1 - 2 * (x * x + ysqr);
|
||||
euler.z = std::atan2(t0, t1);
|
||||
|
||||
float t2 = 2 * (w * y - z * x);
|
||||
t2 = t2 > 1 ? 1 : t2;
|
||||
t2 = t2 < -1 ? -1 : t2;
|
||||
euler.y = std::asin(t2);
|
||||
|
||||
float t3 = 2 * (w * z + x * y);
|
||||
float t4 = 1 - 2 * (ysqr + z * z);
|
||||
euler.x = std::atan2(t3, t4);
|
||||
|
||||
return euler;
|
||||
}
|
||||
*/
|
||||
|
||||
Mat4<T> ToMatrix() const
|
||||
{
|
||||
Mat4<T> result;
|
||||
|
||||
T x2 = x + x;
|
||||
T y2 = y + y;
|
||||
T z2 = z + z;
|
||||
T x2w = x2 * w;
|
||||
T y2w = y2 * w;
|
||||
T z2w = z2 * w;
|
||||
T x2x = x2 * x;
|
||||
T y2x = y2 * x;
|
||||
T z2x = z2 * x;
|
||||
T y2y = y2 * y;
|
||||
T z2y = z2 * y;
|
||||
T z2z = z2 * y;
|
||||
|
||||
result[0] = T(1) - (y2y + z2z);
|
||||
result[1] = y2x - z2w;
|
||||
result[2] = z2x + y2w;
|
||||
result[3] = T(0);
|
||||
|
||||
result[4] = y2x + z2w;
|
||||
result[5] = T(1) - (x2x + z2z);
|
||||
result[6] = z2y - x2w;
|
||||
result[7] = T(0);
|
||||
|
||||
result[8] = z2x - y2w;
|
||||
result[9] = z2y + x2w;
|
||||
result[10] = T(1) - (x2x + y2y);
|
||||
result[11] = T(0);
|
||||
|
||||
result[12] = T(0);
|
||||
result[13] = T(0);
|
||||
result[14] = T(0);
|
||||
result[15] = T(1);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
float GetMagnitude()
|
||||
{
|
||||
return Math::Sqrt<T>(Math::Pow<T>(w, 2) + Math::Pow<T>(x, 2) + Math::Pow<T>(y, 2) + Math::Pow<T>(z, 2));
|
||||
}
|
||||
|
||||
Quat<T> GetNormalized()
|
||||
{
|
||||
T mag = GetMagnitude();
|
||||
|
||||
return Quat<T>(w / mag, x / mag, y / mag, z / mag);
|
||||
}
|
||||
|
||||
void Normalize()
|
||||
{
|
||||
T mag = GetMagnitude();
|
||||
|
||||
w = w / mag;
|
||||
x = x / mag;
|
||||
y = y / mag;
|
||||
z = z / mag;
|
||||
}
|
||||
|
||||
T Dot(const Quat& other) const
|
||||
{
|
||||
return w * other.w + x * other.x + y * other.y + z * other.z;
|
||||
}
|
||||
|
||||
Quat<T> GetConjugate()
|
||||
{
|
||||
return Quat<T>(w, -x, -y, -z);
|
||||
}
|
||||
|
||||
void Conjugate()
|
||||
{
|
||||
x = -x;
|
||||
y = -y;
|
||||
z = -z;
|
||||
}
|
||||
|
||||
Quat<T> GetInverse()
|
||||
{
|
||||
return Quat<T>(w, -x, -y, -z);
|
||||
}
|
||||
|
||||
void Inverse()
|
||||
{
|
||||
x = -x;
|
||||
y = -y;
|
||||
z = -z;
|
||||
}
|
||||
|
||||
static Quat<T> Slerp(Quat<T> start, Quat<T> finish, const T t)
|
||||
{
|
||||
T cosHalfTheta = start.Dot(finish);
|
||||
if (Math::Abs(cosHalfTheta) >= 1.0f)
|
||||
return start;
|
||||
|
||||
float halfTheta = Math::ACos(cosHalfTheta);
|
||||
float sinHalfTheta = Math::Sqrt(1.0f - cosHalfTheta * cosHalfTheta);
|
||||
if (Math::Abs(sinHalfTheta) < 0.001f)
|
||||
{
|
||||
return {
|
||||
start.w * 0.5f + finish.w * 0.5f,
|
||||
start.x * 0.5f + finish.x * 0.5f,
|
||||
start.y * 0.5f + finish.y * 0.5f,
|
||||
start.z * 0.5f + finish.z * 0.5f
|
||||
};
|
||||
}
|
||||
|
||||
float ratioA = Math::Sin((1 - t) * halfTheta) / sinHalfTheta;
|
||||
float ratioB = Math::Sin(t * halfTheta) / sinHalfTheta;
|
||||
|
||||
return {
|
||||
start.w * ratioA + finish.w * ratioB,
|
||||
start.x * ratioA + finish.x * ratioB,
|
||||
start.y * ratioA + finish.y * ratioB,
|
||||
start.z * ratioA + finish.z * ratioB
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
typedef Quat<float> Quat_f;
|
||||
}
|
||||
Reference in New Issue
Block a user