commit x64 compilation from lulu cause the other branch dont seems to compile properly at home
This commit is contained in:
218
include/framework/math/quaternion.cpp
Normal file
218
include/framework/math/quaternion.cpp
Normal file
@ -0,0 +1,218 @@
|
||||
/* -----------------------------------------------------------------------------
|
||||
GSFramework
|
||||
Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
|
||||
----------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
#include "math/quaternion.h"
|
||||
#include "math/matrix3.h"
|
||||
#include "metafile/nml.h"
|
||||
|
||||
using namespace GS;
|
||||
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Quaternion Quaternion::Slerp(float t, const Quaternion &a, const Quaternion &b)
|
||||
{
|
||||
float norm = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
||||
|
||||
bool bFlip = false;
|
||||
|
||||
if (norm < 0.0f)
|
||||
{
|
||||
norm = -norm;
|
||||
bFlip = true;
|
||||
}
|
||||
|
||||
float inv_d;
|
||||
if (1.0f - norm < 0.000001f)
|
||||
inv_d = 1.0f - t;
|
||||
|
||||
else
|
||||
{
|
||||
float theta = Math::ACos(norm);
|
||||
float s = 1.f / Math::Sin(theta);
|
||||
|
||||
inv_d = Math::Sin((1.0f - t) * theta) * s;
|
||||
t = Math::Sin(t * theta) * s;
|
||||
}
|
||||
|
||||
if (bFlip)
|
||||
t = -t;
|
||||
|
||||
return Quaternion(inv_d * a.x + t * b.x, inv_d * a.y + t * b.y, inv_d * a.z + t * b.z, inv_d * a.w + t * b.w);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
float Quaternion::Distance(const Quaternion &a, const Quaternion &b)
|
||||
{
|
||||
const float dx = a.x - b.x, dy = a.y - b.y, dz = a.z - b.z, dw = a.w - b.w;
|
||||
return Math::Sqrt((dx * dx) + (dy * dy) + (dz * dz) + (dw * dw));
|
||||
}
|
||||
Quaternion Quaternion::Inverse() const
|
||||
{
|
||||
const float norm = w * w + x * x + y * y + z * z;
|
||||
if (norm > 0)
|
||||
{
|
||||
const float inorm = 1.f / norm;
|
||||
return Quaternion(x * -inorm, y * -inorm, z * -inorm, w * inorm);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
Quaternion Quaternion::Normalize() const
|
||||
{
|
||||
float d = Math::Sqrt(x * x + y * y + z * z + w * w);
|
||||
if (!d)
|
||||
return Quaternion(1, 1, 1, 1);
|
||||
|
||||
float k = 1.f / d;
|
||||
return Quaternion(x * k, y * k, z * k, w * k);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Quaternion Quaternion::LookAt(const Vector4 &at)
|
||||
{ return Quaternion::FromMatrix3(Matrix3::FromOrthonormalBasis(at)); }
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Quaternion Quaternion::FromMatrix3(const Matrix3 &m)
|
||||
{
|
||||
// From "Quaternion Calculus and Fast Animation".
|
||||
float x, y, z, w;
|
||||
float trace = m.m[0][0] + m.m[1][1] + m.m[2][2];
|
||||
|
||||
if (trace > 0.0)
|
||||
{
|
||||
// |w| > 1/2, may as well choose w > 1/2
|
||||
float root = Math::Sqrt(trace + 1.0f); // 2w
|
||||
w = 0.5f * root;
|
||||
root = 0.5f / root; // 1/(4w)
|
||||
x = (m.m[2][1] - m.m[1][2]) * root;
|
||||
y = (m.m[0][2] - m.m[2][0]) * root;
|
||||
z = (m.m[1][0] - m.m[0][1]) * root;
|
||||
}
|
||||
else
|
||||
{
|
||||
// |w| <= 1/2
|
||||
static size_t inext[3] = { 1, 2, 0 };
|
||||
size_t i = 0;
|
||||
if (m.m[1][1] > m.m[0][0])
|
||||
i = 1;
|
||||
if (m.m[2][2] > m.m[i][i])
|
||||
i = 2;
|
||||
size_t j = inext[i];
|
||||
size_t k = inext[j];
|
||||
|
||||
float root = Math::Sqrt(m.m[i][i] - m.m[j][j] - m.m[k][k] + 1.0f);
|
||||
float *quat[3] = { &x, &y, &z };
|
||||
*quat[i] = 0.5f * root;
|
||||
root = 0.5f / root;
|
||||
w = (m.m[k][j] - m.m[j][k]) * root;
|
||||
*quat[j] = (m.m[j][i] + m.m[i][j]) * root;
|
||||
*quat[k] = (m.m[k][i] + m.m[i][k]) * root;
|
||||
}
|
||||
return Quaternion(x, y, z, w);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Quaternion Quaternion::FromAxisAngle(float a, float _x, float _y, float _z)
|
||||
{
|
||||
float sn = Math::Sin(a * 0.5f), cs = Math::Cos(a * 0.5f);
|
||||
return Quaternion(_x * sn, _y * sn, _z * sn, cs).Normalize();
|
||||
}
|
||||
Quaternion Quaternion::FromEuler(float _x, float _y, float _z, Math::rOrder rorder)
|
||||
{
|
||||
Quaternion qx(Quaternion::FromAxisAngle(_x, 1, 0, 0)),
|
||||
qy(Quaternion::FromAxisAngle(_y, 0, 1, 0)),
|
||||
qz(Quaternion::FromAxisAngle(_z, 0, 0, 1)),
|
||||
q;
|
||||
|
||||
switch (rorder)
|
||||
{
|
||||
case Math::rOrder_ZYX: q = qz * qy * qx; break;
|
||||
case Math::rOrder_YZX: q = qy * qz * qx; break;
|
||||
case Math::rOrder_ZXY: q = qz * qx * qy; break;
|
||||
case Math::rOrder_XZY: q = qx * qz * qy; break;
|
||||
default:
|
||||
case Math::rOrder_YXZ: q = qy * qx * qz; break;
|
||||
case Math::rOrder_XYZ: q = qx * qy * qz; break;
|
||||
case Math::rOrder_XY: q = qx * qy; break;
|
||||
}
|
||||
return q.Normalize();
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix3 Quaternion::AsMatrix3() const
|
||||
{
|
||||
float sqw = w * w, sqx = x * x, sqy = y * y, sqz = z * z;
|
||||
|
||||
Matrix3 m;
|
||||
|
||||
float invs = 1.f / (sqx + sqy + sqz + sqw);
|
||||
m.m[0][0] = ( sqx - sqy - sqz + sqw) * invs; // Since sqw + sqx + sqy + sqz = 1 / invs * invs.
|
||||
m.m[1][1] = (-sqx + sqy - sqz + sqw) * invs;
|
||||
m.m[2][2] = (-sqx - sqy + sqz + sqw) * invs;
|
||||
|
||||
float tmp1 = x * y;
|
||||
float tmp2 = z * w;
|
||||
m.m[1][0] = 2.f * (tmp1 + tmp2) * invs;
|
||||
m.m[0][1] = 2.f * (tmp1 - tmp2) * invs;
|
||||
|
||||
tmp1 = x * z;
|
||||
tmp2 = y * w;
|
||||
m.m[2][0] = 2.f * (tmp1 - tmp2) * invs;
|
||||
m.m[0][2] = 2.f * (tmp1 + tmp2) * invs;
|
||||
tmp1 = y * z;
|
||||
tmp2 = x * w;
|
||||
m.m[2][1] = 2.f * (tmp1 + tmp2) * invs;
|
||||
m.m[1][2] = 2.f * (tmp1 - tmp2) * invs;
|
||||
|
||||
return m;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
NML::Tag *Quaternion::AsMetaTag(const char *id) const
|
||||
{
|
||||
NML::Tag *root = new NML::Tag(id ? id : "Quaternion");
|
||||
|
||||
if (root)
|
||||
{
|
||||
root->AddChild("X", x);
|
||||
root->AddChild("Y", y);
|
||||
root->AddChild("Z", z);
|
||||
root->AddChild("W", w);
|
||||
}
|
||||
return root;
|
||||
}
|
||||
bool Quaternion::FromMetaTag(NML::Tag &tag)
|
||||
{
|
||||
NML::Tag *t;
|
||||
List <NML::Tag *> ::Iterator i(tag.GetTags().GetRoot());
|
||||
|
||||
t = i.ObjectPtr();
|
||||
if (!t) return false;
|
||||
x = t->GetReal();
|
||||
++i;
|
||||
|
||||
t = i.ObjectPtr();
|
||||
if (!t) return false;
|
||||
y = t->GetReal();
|
||||
++i;
|
||||
|
||||
t = i.ObjectPtr();
|
||||
if (!t) return false;
|
||||
z = t->GetReal();
|
||||
++i;
|
||||
|
||||
t = i.ObjectPtr();
|
||||
if (!t) return false;
|
||||
w = t->GetReal();
|
||||
|
||||
return true;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
Reference in New Issue
Block a user