112 lines
1.9 KiB
C++
112 lines
1.9 KiB
C++
#include "../../include/math/Quaternion.h"
|
|
#include "../../include/math/Math.h"
|
|
#include "../../include/math/vec3.h"
|
|
|
|
Quaternion Quaternion::identity;
|
|
|
|
|
|
Quaternion::Quaternion()
|
|
: x{ 0 }
|
|
, y{ 0 }
|
|
, z{ 0 }
|
|
, w{ 1 }
|
|
{
|
|
}
|
|
|
|
|
|
Quaternion Quaternion::CreateFromAxisAngle(const vec3& axis, float angle)
|
|
{
|
|
float halfAngle = angle * .5f;
|
|
float s = (float)Math::Sin(halfAngle);
|
|
Quaternion q;
|
|
q.x = axis.x * s;
|
|
q.y = axis.y * s;
|
|
q.z = axis.z * s;
|
|
q.w = (float)Math::Cos(halfAngle);
|
|
return q;
|
|
}
|
|
|
|
|
|
Quaternion Quaternion::LookRotation(const vec3& forward)
|
|
{
|
|
float dot = vec3::Dot(vec3::up, forward);
|
|
|
|
if (Math::Abs(dot - (-1.0f)) < 0.000001f)
|
|
{
|
|
return CreateFromAxisAngle(vec3::forward, Math::Pi);
|
|
}
|
|
if (Math::Abs(dot - (1.0f)) < 0.000001f)
|
|
{
|
|
return Quaternion::identity;
|
|
}
|
|
|
|
float rotAngle = (float)Math::Acos(dot);
|
|
vec3 rotAxis = vec3::Cross(vec3::up, forward);
|
|
rotAxis.Normalize();
|
|
|
|
return CreateFromAxisAngle(rotAxis, rotAngle);
|
|
}
|
|
|
|
|
|
Quaternion Quaternion::FromTo(vec3 v0, vec3 v1)
|
|
{
|
|
Quaternion q;
|
|
v0.Normalize();
|
|
v1.Normalize();
|
|
|
|
float d = vec3::Dot(v0, v1);
|
|
// If dot == 1, vectors are the same
|
|
if (d >= 1.0f)
|
|
{
|
|
return Quaternion::identity;
|
|
}
|
|
if (d < (1e-6f - 1.0f))
|
|
{
|
|
vec3 axis = vec3::Cross(vec3::right, v0);
|
|
if (axis.GetLengthSq() < Math::FloatEpsilon) // pick another if colinear
|
|
axis = vec3::Cross(vec3::up, v0);
|
|
axis.Normalize();
|
|
return Quaternion::CreateFromAxisAngle(axis, Math::Pi);
|
|
}
|
|
else
|
|
{
|
|
auto s = Math::Sqrt((1 + d) * 2);
|
|
auto invs = 1 / s;
|
|
|
|
auto c = vec3::Cross(v0, v1);
|
|
|
|
q.x = c.x * invs;
|
|
q.y = c.y * invs;
|
|
q.z = c.z * invs;
|
|
q.w = s * 0.5f;
|
|
q.Normalize();
|
|
}
|
|
return q;
|
|
}
|
|
|
|
|
|
void Quaternion::Inverse()
|
|
{
|
|
auto newX = w;
|
|
auto newY = -x;
|
|
auto newZ = -y;
|
|
auto newW = -z;
|
|
x = newX;
|
|
y = newY;
|
|
z = newZ;
|
|
w = newW;
|
|
|
|
Normalize();
|
|
}
|
|
|
|
|
|
void Quaternion::Normalize()
|
|
{
|
|
auto norm = w*w + x*x + y*y + z*z;
|
|
auto s = 1 / norm;
|
|
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
w *= s;
|
|
} |