AxioDL/zeus
2
0
Fork 0
mirror of https://github.com/AxioDL/zeus.git synced 2025-12-10 22:17:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

CQuaternion fixes

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

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;
}
}