344 lines
8.0 KiB
Plaintext
344 lines
8.0 KiB
Plaintext
|
/// @ref gtx_pca
|
||
|
|
|
||
|
|
#ifndef GLM_HAS_CXX11_STL
|
||
|
|
#include <algorithm>
|
||
|
|
#else
|
||
|
|
#include <utility>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
namespace glm {
|
||
|
|
|
||
|
|
|
||
|
|
template<length_t D, typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER mat<D, D, T, Q> computeCovarianceMatrix(vec<D, T, Q> const* v, size_t n)
|
||
|
|
{
|
||
|
|
return computeCovarianceMatrix<D, T, Q, vec<D, T, Q> const*>(v, v + n);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<length_t D, typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER mat<D, D, T, Q> computeCovarianceMatrix(vec<D, T, Q> const* v, size_t n, vec<D, T, Q> const& c)
|
||
|
|
{
|
||
|
|
return computeCovarianceMatrix<D, T, Q, vec<D, T, Q> const*>(v, v + n, c);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<length_t D, typename T, qualifier Q, typename I>
|
||
|
|
GLM_FUNC_QUALIFIER mat<D, D, T, Q> computeCovarianceMatrix(I const& b, I const& e)
|
||
|
|
{
|
||
|
|
glm::mat<D, D, T, Q> m(0);
|
||
|
|
|
||
|
|
size_t cnt = 0;
|
||
|
|
for(I i = b; i != e; i++)
|
||
|
|
{
|
||
|
|
vec<D, T, Q> const& v = *i;
|
||
|
|
for(length_t x = 0; x < D; ++x)
|
||
|
|
for(length_t y = 0; y < D; ++y)
|
||
|
|
m[x][y] += static_cast<T>(v[x] * v[y]);
|
||
|
|
cnt++;
|
||
|
|
}
|
||
|
|
if(cnt > 0)
|
||
|
|
m /= static_cast<T>(cnt);
|
||
|
|
|
||
|
|
return m;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<length_t D, typename T, qualifier Q, typename I>
|
||
|
|
GLM_FUNC_QUALIFIER mat<D, D, T, Q> computeCovarianceMatrix(I const& b, I const& e, vec<D, T, Q> const& c)
|
||
|
|
{
|
||
|
|
glm::mat<D, D, T, Q> m(0);
|
||
|
|
glm::vec<D, T, Q> v;
|
||
|
|
|
||
|
|
size_t cnt = 0;
|
||
|
|
for(I i = b; i != e; i++)
|
||
|
|
{
|
||
|
|
v = *i - c;
|
||
|
|
for(length_t x = 0; x < D; ++x)
|
||
|
|
for(length_t y = 0; y < D; ++y)
|
||
|
|
m[x][y] += static_cast<T>(v[x] * v[y]);
|
||
|
|
cnt++;
|
||
|
|
}
|
||
|
|
if(cnt > 0)
|
||
|
|
m /= static_cast<T>(cnt);
|
||
|
|
|
||
|
|
return m;
|
||
|
|
}
|
||
|
|
|
||
|
|
namespace _internal_
|
||
|
|
{
|
||
|
|
|
||
|
|
template<typename T>
|
||
|
|
GLM_FUNC_QUALIFIER static T transferSign(T const& v, T const& s)
|
||
|
|
{
|
||
|
|
return ((s) >= 0 ? glm::abs(v) : -glm::abs(v));
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename T>
|
||
|
|
GLM_FUNC_QUALIFIER static T pythag(T const& a, T const& b) {
|
||
|
|
static const T epsilon = static_cast<T>(0.0000001);
|
||
|
|
T absa = glm::abs(a);
|
||
|
|
T absb = glm::abs(b);
|
||
|
|
if(absa > absb) {
|
||
|
|
absb /= absa;
|
||
|
|
absb *= absb;
|
||
|
|
return absa * glm::sqrt(static_cast<T>(1) + absb);
|
||
|
|
}
|
||
|
|
if(glm::equal<T>(absb, 0, epsilon)) return static_cast<T>(0);
|
||
|
|
absa /= absb;
|
||
|
|
absa *= absa;
|
||
|
|
return absb * glm::sqrt(static_cast<T>(1) + absa);
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
template<length_t D, typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER unsigned int findEigenvaluesSymReal
|
||
|
|
(
|
||
|
|
mat<D, D, T, Q> const& covarMat,
|
||
|
|
vec<D, T, Q>& outEigenvalues,
|
||
|
|
mat<D, D, T, Q>& outEigenvectors
|
||
|
|
)
|
||
|
|
{
|
||
|
|
using _internal_::transferSign;
|
||
|
|
using _internal_::pythag;
|
||
|
|
|
||
|
|
T a[D * D]; // matrix -- input and workspace for algorithm (will be changed inplace)
|
||
|
|
T d[D]; // diagonal elements
|
||
|
|
T e[D]; // off-diagonal elements
|
||
|
|
|
||
|
|
for(length_t r = 0; r < D; r++)
|
||
|
|
for(length_t c = 0; c < D; c++)
|
||
|
|
a[(r) * D + (c)] = covarMat[c][r];
|
||
|
|
|
||
|
|
// 1. Householder reduction.
|
||
|
|
length_t l, k, j, i;
|
||
|
|
T scale, hh, h, g, f;
|
||
|
|
static const T epsilon = static_cast<T>(0.0000001);
|
||
|
|
|
||
|
|
for(i = D; i >= 2; i--)
|
||
|
|
{
|
||
|
|
l = i - 1;
|
||
|
|
h = scale = 0;
|
||
|
|
if(l > 1)
|
||
|
|
{
|
||
|
|
for(k = 1; k <= l; k++)
|
||
|
|
{
|
||
|
|
scale += glm::abs(a[(i - 1) * D + (k - 1)]);
|
||
|
|
}
|
||
|
|
if(glm::equal<T>(scale, 0, epsilon))
|
||
|
|
{
|
||
|
|
e[i - 1] = a[(i - 1) * D + (l - 1)];
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for(k = 1; k <= l; k++)
|
||
|
|
{
|
||
|
|
a[(i - 1) * D + (k - 1)] /= scale;
|
||
|
|
h += a[(i - 1) * D + (k - 1)] * a[(i - 1) * D + (k - 1)];
|
||
|
|
}
|
||
|
|
f = a[(i - 1) * D + (l - 1)];
|
||
|
|
g = ((f >= 0) ? -glm::sqrt(h) : glm::sqrt(h));
|
||
|
|
e[i - 1] = scale * g;
|
||
|
|
h -= f * g;
|
||
|
|
a[(i - 1) * D + (l - 1)] = f - g;
|
||
|
|
f = 0;
|
||
|
|
for(j = 1; j <= l; j++)
|
||
|
|
{
|
||
|
|
a[(j - 1) * D + (i - 1)] = a[(i - 1) * D + (j - 1)] / h;
|
||
|
|
g = 0;
|
||
|
|
for(k = 1; k <= j; k++)
|
||
|
|
{
|
||
|
|
g += a[(j - 1) * D + (k - 1)] * a[(i - 1) * D + (k - 1)];
|
||
|
|
}
|
||
|
|
for(k = j + 1; k <= l; k++)
|
||
|
|
{
|
||
|
|
g += a[(k - 1) * D + (j - 1)] * a[(i - 1) * D + (k - 1)];
|
||
|
|
}
|
||
|
|
e[j - 1] = g / h;
|
||
|
|
f += e[j - 1] * a[(i - 1) * D + (j - 1)];
|
||
|
|
}
|
||
|
|
hh = f / (h + h);
|
||
|
|
for(j = 1; j <= l; j++)
|
||
|
|
{
|
||
|
|
f = a[(i - 1) * D + (j - 1)];
|
||
|
|
e[j - 1] = g = e[j - 1] - hh * f;
|
||
|
|
for(k = 1; k <= j; k++)
|
||
|
|
{
|
||
|
|
a[(j - 1) * D + (k - 1)] -= (f * e[k - 1] + g * a[(i - 1) * D + (k - 1)]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
e[i - 1] = a[(i - 1) * D + (l - 1)];
|
||
|
|
}
|
||
|
|
d[i - 1] = h;
|
||
|
|
}
|
||
|
|
d[0] = 0;
|
||
|
|
e[0] = 0;
|
||
|
|
for(i = 1; i <= D; i++)
|
||
|
|
{
|
||
|
|
l = i - 1;
|
||
|
|
if(!glm::equal<T>(d[i - 1], 0, epsilon))
|
||
|
|
{
|
||
|
|
for(j = 1; j <= l; j++)
|
||
|
|
{
|
||
|
|
g = 0;
|
||
|
|
for(k = 1; k <= l; k++)
|
||
|
|
{
|
||
|
|
g += a[(i - 1) * D + (k - 1)] * a[(k - 1) * D + (j - 1)];
|
||
|
|
}
|
||
|
|
for(k = 1; k <= l; k++)
|
||
|
|
{
|
||
|
|
a[(k - 1) * D + (j - 1)] -= g * a[(k - 1) * D + (i - 1)];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
d[i - 1] = a[(i - 1) * D + (i - 1)];
|
||
|
|
a[(i - 1) * D + (i - 1)] = 1;
|
||
|
|
for(j = 1; j <= l; j++)
|
||
|
|
{
|
||
|
|
a[(j - 1) * D + (i - 1)] = a[(i - 1) * D + (j - 1)] = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// 2. Calculation of eigenvalues and eigenvectors (QL algorithm)
|
||
|
|
length_t m, iter;
|
||
|
|
T s, r, p, dd, c, b;
|
||
|
|
const length_t MAX_ITER = 30;
|
||
|
|
|
||
|
|
for(i = 2; i <= D; i++)
|
||
|
|
{
|
||
|
|
e[i - 2] = e[i - 1];
|
||
|
|
}
|
||
|
|
e[D - 1] = 0;
|
||
|
|
|
||
|
|
for(l = 1; l <= D; l++)
|
||
|
|
{
|
||
|
|
iter = 0;
|
||
|
|
do
|
||
|
|
{
|
||
|
|
for(m = l; m <= D - 1; m++)
|
||
|
|
{
|
||
|
|
dd = glm::abs(d[m - 1]) + glm::abs(d[m - 1 + 1]);
|
||
|
|
if(glm::equal<T>(glm::abs(e[m - 1]) + dd, dd, epsilon))
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if(m != l)
|
||
|
|
{
|
||
|
|
if(iter++ == MAX_ITER)
|
||
|
|
{
|
||
|
|
return 0; // Too many iterations in FindEigenvalues
|
||
|
|
}
|
||
|
|
g = (d[l - 1 + 1] - d[l - 1]) / (2 * e[l - 1]);
|
||
|
|
r = pythag<T>(g, 1);
|
||
|
|
g = d[m - 1] - d[l - 1] + e[l - 1] / (g + transferSign(r, g));
|
||
|
|
s = c = 1;
|
||
|
|
p = 0;
|
||
|
|
for(i = m - 1; i >= l; i--)
|
||
|
|
{
|
||
|
|
f = s * e[i - 1];
|
||
|
|
b = c * e[i - 1];
|
||
|
|
e[i - 1 + 1] = r = pythag(f, g);
|
||
|
|
if(glm::equal<T>(r, 0, epsilon))
|
||
|
|
{
|
||
|
|
d[i - 1 + 1] -= p;
|
||
|
|
e[m - 1] = 0;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
s = f / r;
|
||
|
|
c = g / r;
|
||
|
|
g = d[i - 1 + 1] - p;
|
||
|
|
r = (d[i - 1] - g) * s + 2 * c * b;
|
||
|
|
d[i - 1 + 1] = g + (p = s * r);
|
||
|
|
g = c * r - b;
|
||
|
|
for(k = 1; k <= D; k++)
|
||
|
|
{
|
||
|
|
f = a[(k - 1) * D + (i - 1 + 1)];
|
||
|
|
a[(k - 1) * D + (i - 1 + 1)] = s * a[(k - 1) * D + (i - 1)] + c * f;
|
||
|
|
a[(k - 1) * D + (i - 1)] = c * a[(k - 1) * D + (i - 1)] - s * f;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if(glm::equal<T>(r, 0, epsilon) && (i >= l))
|
||
|
|
continue;
|
||
|
|
d[l - 1] -= p;
|
||
|
|
e[l - 1] = g;
|
||
|
|
e[m - 1] = 0;
|
||
|
|
}
|
||
|
|
} while(m != l);
|
||
|
|
}
|
||
|
|
|
||
|
|
// 3. output
|
||
|
|
for(i = 0; i < D; i++)
|
||
|
|
outEigenvalues[i] = d[i];
|
||
|
|
for(i = 0; i < D; i++)
|
||
|
|
for(j = 0; j < D; j++)
|
||
|
|
outEigenvectors[i][j] = a[(j) * D + (i)];
|
||
|
|
|
||
|
|
return D;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER void sortEigenvalues(vec<2, T, Q>& eigenvalues, mat<2, 2, T, Q>& eigenvectors)
|
||
|
|
{
|
||
|
|
if (eigenvalues[0] < eigenvalues[1])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[0], eigenvalues[1]);
|
||
|
|
std::swap(eigenvectors[0], eigenvectors[1]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER void sortEigenvalues(vec<3, T, Q>& eigenvalues, mat<3, 3, T, Q>& eigenvectors)
|
||
|
|
{
|
||
|
|
if (eigenvalues[0] < eigenvalues[1])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[0], eigenvalues[1]);
|
||
|
|
std::swap(eigenvectors[0], eigenvectors[1]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[0] < eigenvalues[2])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[0], eigenvalues[2]);
|
||
|
|
std::swap(eigenvectors[0], eigenvectors[2]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[1] < eigenvalues[2])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[1], eigenvalues[2]);
|
||
|
|
std::swap(eigenvectors[1], eigenvectors[2]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<typename T, qualifier Q>
|
||
|
|
GLM_FUNC_QUALIFIER void sortEigenvalues(vec<4, T, Q>& eigenvalues, mat<4, 4, T, Q>& eigenvectors)
|
||
|
|
{
|
||
|
|
if (eigenvalues[0] < eigenvalues[2])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[0], eigenvalues[2]);
|
||
|
|
std::swap(eigenvectors[0], eigenvectors[2]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[1] < eigenvalues[3])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[1], eigenvalues[3]);
|
||
|
|
std::swap(eigenvectors[1], eigenvectors[3]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[0] < eigenvalues[1])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[0], eigenvalues[1]);
|
||
|
|
std::swap(eigenvectors[0], eigenvectors[1]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[2] < eigenvalues[3])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[2], eigenvalues[3]);
|
||
|
|
std::swap(eigenvectors[2], eigenvectors[3]);
|
||
|
|
}
|
||
|
|
if (eigenvalues[1] < eigenvalues[2])
|
||
|
|
{
|
||
|
|
std::swap(eigenvalues[1], eigenvalues[2]);
|
||
|
|
std::swap(eigenvectors[1], eigenvectors[2]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}//namespace glm
|