bool DiagonalizeMatrix(const mat3& m, mat3& res)
{
vec3 v;
if (!EigenValues(m, v))
return false;
res.Identity();
res[0] = v[0];
res[4] = v[1];
res[8] = v[2];
return true;
}
bool SymmetricEigenSystem(const mat3& m, vec3& value, mat3& r)
{
const int max_sweep = 32;
const float epsilon = 1.0e-10f;
float m11 = m.At(0,0);
float m12 = m.At(0,1);
float m13 = m.At(0,2);
float m22 = m.At(1,1);
float m23 = m.At(1,2);
float m33 = m.At(2,2);
r.Identity();
for (int a = 0; a < max_sweep; ++a)
{
if ((Abs(m12) < epsilon) && (Abs(m13) < epsilon) && (Abs(m23) < epsilon))
break;
// annihilate 1 2
if (m12 != 0.0f)
{
float u = (m22 - m11) * 0.5f / m12;
float u2 = u*u;
float u2p1 = u2 + 1.0f;
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
float c = 1.0f / sqrt(t*t + 1.0f);
float s = c * t;
m11 -= t * m12;
m22 += t * m12;
m12 = 0.0f;
float temp = c*m13 - s * m23;
m23 = s * m13 + c * m23;
m13 = temp;
for (int i = 0; i < 3; ++i)
{
float temp = c * r.At(i, 0) - s * r.At(i,1);
r.At(i,1) = s * r.At(i,0) + c * r.At(i,1);
r.At(i,0) = temp;
}
}
// annihilate 1 3
if (m13 != 0.0f)
{
float u = (m33 - m11) * 0.5f / m13;
float u2 = u*u;
float u2p1 = u2 + 1.0f;
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
float c = 1.0f / sqrt(t*t + 1.0f);
float s = c * t;
m11 -= t * m13;
m33 += t * m13;
m13 = 0.0f;
float temp = c * m12 - s * m23;
m23 = s * m12 + c * m23;
m12 = temp;
for (int i = 0; i < 3; ++i)
{
float temp = c * r.At(i, 0) - s * r.At(i, 2);
r.At(i, 2) = s * r.At(i, 0) + c * r.At(i, 2);
r.At(i, 0) = temp;
}
}
// annihilate 2 3
if (m23 != 0.0f)
{
float u = (m33 - m22) * 0.5f / m23;
float u2 = u*u;
float u2p1 = u2 + 1.0f;
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
float c = 1.0f / sqrt(t*t + 1.0f);
float s = c * t;
m22 -= t * m23;
m33 += t * m23;
m23 = 0.0f;
float temp = c * m12 - s * m13;
m13 = s * m12 + c * m13;
m12 = temp;
for (int i = 0; i < 3; ++i)
{
float temp = c * r.At(i, 1) - s * r.At(i, 2);
r.At(i, 2) = s * r.At(i, 1) + c * r.At(i, 2);
r.At(i, 1) = temp;
}
}
}
value.Set(m11, m22, m33);
return true;
}
