AxioDL
/
zeus
mirror of https://github.com/AxioDL/zeus.git
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

CQuaternion fixes

This commit is contained in:
Jack Andersen 2017-12-15 14:16:40 -10:00
parent b359ff96d0
commit b438e30060
3 changed files with 107 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
include/zeus/CQuaternion.hpp
View File

@ -19,9 +19,9 @@ namespace zeus
static inline float normalize_angle(float angle)
{
if (angle > M_PIF)
angle = -((2.f * angle) - M_PIF);
angle -= 2.f * M_PIF;
else if (angle < -M_PIF)
angle = 2.f * angle + M_PIF;
angle += 2.f * M_PIF;
return angle;
}
@ -90,74 +90,7 @@ public:
#endif
CQuaternion(const CMatrix3f& mat)
{
float trace = mat[0][0] + mat[1][1] + mat[2][2];
if (trace >= 0.f)
{
float st = std::sqrt(trace + 1.0f);
float s = 0.5f / st;
w = 0.5f * st;
x = (mat[1][2] - mat[2][1]) * s;
y = (mat[2][0] - mat[0][2]) * s;
z = (mat[0][1] - mat[1][0]) * s;
}
else
{
int idx = 0;
if (mat[1][1] > mat[0][0])
{
idx = 1;
if (mat[2][2] > mat[1][1])
idx = 2;
}
else if (mat[2][2] > mat[0][0])
{
idx = 2;
}
switch (idx)
{
case 0:
{
float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
float s = 0.5f / st;
w = (mat[1][2] - mat[2][1]) * s;
x = 0.5f * st;
y = (mat[1][0] + mat[0][1]) * s;
z = (mat[2][0] + mat[0][2]) * s;
break;
}
case 1:
{
float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f);
float s = 0.5f / st;
w = (mat[2][0] - mat[0][2]) * s;
x = (mat[1][0] + mat[0][1]) * s;
y = 0.5f * st;
z = (mat[2][1] + mat[1][2]) * s;
break;
}
case 2:
{
float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f);
float s = 0.5f / st;
w = (mat[0][1] - mat[1][0]) * s;
x = (mat[2][0] + mat[0][2]) * s;
y = (mat[2][1] + mat[1][2]) * s;
z = 0.5f * st;
break;
}
default:
w = 0.f;
x = 0.f;
y = 0.f;
z = 0.f;
break;
}
}
}
CQuaternion(const CMatrix3f& mat);
CQuaternion(const CVector3f& vec) { fromVector3f(vec); }
CQuaternion(const CVector4f& vec)
{
@ -224,40 +157,7 @@ public:
return {q.x, q.y, q.z};
}
static inline CQuaternion lookAt(const CUnitVector3f& target, const CUnitVector3f& up, const CRelAngle& c)
{
CQuaternion q = skNoRotation;
zeus::CVector3f upCpy = up;
zeus::CVector3f targetCpy = target;
upCpy.z = 0.f;
targetCpy.z = 0.f;
zeus::CVector3f tmp;
if (upCpy.magnitude() > 0.0009f && upCpy.magnitude() > 0.0009f)
{
targetCpy.normalize();
upCpy.normalize();
CRelAngle angleBetween =
normalize_angle(std::atan2(targetCpy.x, targetCpy.y) - std::atan2(upCpy.x, upCpy.y));
CRelAngle realAngle = zeus::clamp<CRelAngle>(-c, angleBetween, c);
CQuaternion tmpQ;
tmpQ.rotateZ(realAngle);
q = tmpQ;
CQuaternion q2 = (q * CQuaternion{0.f, targetCpy}) * -tmpQ;
tmp.x = q2.x;
tmp.y = q2.y;
tmp.z = q2.z;
}
else if (upCpy.magnitude() > 0.0009f)
tmp = targetCpy.normalized();
else if (upCpy.magnitude() > 0.0009f)
tmp = upCpy.normalized();
else
return skNoRotation;
CRelAngle realAngle = zeus::clamp<CRelAngle>(-c, normalize_angle(std::acos(up.z) - std::acos(target.z)), c);
return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), realAngle) * q;
}
static CQuaternion lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng);
CVector3f transform(const CVector3f& v) const
{

2
include/zeus/CRelAngle.hpp
View File

@ -17,6 +17,8 @@ struct CRelAngle
static float MakeRelativeAngle(float angle)
{
float absAngle = std::fabs(angle);
if (absAngle == 2.f * M_PIF)
return std::copysign(absAngle, angle);
float ret = absAngle - std::floor(absAngle / (2.f * M_PIF)) * (2.f * M_PIF);
if (ret < 0.f)
ret += 2.f * M_PIF;

101
src/CQuaternion.cpp
View File

@ -5,6 +5,74 @@ namespace zeus
{
const CQuaternion CQuaternion::skNoRotation;
CQuaternion::CQuaternion(const CMatrix3f& mat)
{
float trace = mat[0][0] + mat[1][1] + mat[2][2];
if (trace >= 0.f)
{
float st = std::sqrt(trace + 1.0f);
float s = 0.5f / st;
w = 0.5f * st;
x = (mat[1][2] - mat[2][1]) * s;
y = (mat[2][0] - mat[0][2]) * s;
z = (mat[0][1] - mat[1][0]) * s;
}
else
{
int idx = 0;
if (mat[1][1] > mat[0][0])
{
idx = 1;
if (mat[2][2] > mat[1][1])
idx = 2;
}
else if (mat[2][2] > mat[0][0])
{
idx = 2;
}
switch (idx)
{
case 0:
{
float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f);
float s = 0.5f / st;
w = (mat[1][2] - mat[2][1]) * s;
x = 0.5f * st;
y = (mat[1][0] + mat[0][1]) * s;
z = (mat[2][0] + mat[0][2]) * s;
break;
}
case 1:
{
float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f);
float s = 0.5f / st;
w = (mat[2][0] - mat[0][2]) * s;
x = (mat[1][0] + mat[0][1]) * s;
y = 0.5f * st;
z = (mat[2][1] + mat[1][2]) * s;
break;
}
case 2:
{
float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f);
float s = 0.5f / st;
w = (mat[0][1] - mat[1][0]) * s;
x = (mat[2][0] + mat[0][2]) * s;
y = (mat[2][1] + mat[1][2]) * s;
z = 0.5f * st;
break;
}
default:
w = 0.f;
x = 0.f;
y = 0.f;
z = 0.f;
break;
}
}
}
void CQuaternion::fromVector3f(const CVector3f& vec)
{
float cosX = std::cos(0.5f * vec.x);
@ -295,4 +363,37 @@ CRelAngle CQuaternion::angleFrom(const zeus::CQuaternion& other)
return std::acos(zeus::clamp(-1.f, dot(other), 1.f));
}
CQuaternion CQuaternion::lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng)
{
CQuaternion q = skNoRotation;
zeus::CVector3f destNoZ = dest;
zeus::CVector3f sourceNoZ = source;
destNoZ.z = 0.f;
sourceNoZ.z = 0.f;
zeus::CVector3f tmp;
if (sourceNoZ.magSquared() > 0.0001f && destNoZ.magSquared() > 0.0001f)
{
sourceNoZ.normalize();
destNoZ.normalize();
float angleBetween =
normalize_angle(std::atan2(destNoZ.x, destNoZ.y) - std::atan2(sourceNoZ.x, sourceNoZ.y));
float realAngle = zeus::clamp(-maxAng.asRadians(), angleBetween, maxAng.asRadians());
CQuaternion tmpQ;
tmpQ.rotateZ(-realAngle);
q = tmpQ;
tmp = q.transform(sourceNoZ);
}
else if (sourceNoZ.magSquared() > 0.0001f)
tmp = sourceNoZ.normalized();
else if (destNoZ.magSquared() > 0.0001f)
tmp = destNoZ.normalized();
else
return skNoRotation;
float realAngle =
zeus::clamp(-maxAng.asRadians(), normalize_angle(std::acos(dest.z) - std::acos(source.z)), maxAng.asRadians());
return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), -realAngle) * q;
}
}