Merge trunk fixes to 2.0 branch. Get things ready for 2.0.2 release.
castano
16 years ago
parent
46334cb96a
commit
07df6e7539
@ -1,533 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include "Eigen.h"
|
||||
|
||||
using namespace nv;
|
||||
|
||||
static const float EPS = 0.00001f;
|
||||
static const int MAX_ITER = 100;
|
||||
|
||||
static void semi_definite_symmetric_eigen(const float *mat, int n, float *eigen_vec, float *eigen_val);
|
||||
|
||||
|
||||
// Use power method to find the first eigenvector.
|
||||
// http://www.miislita.com/information-retrieval-tutorial/matrix-tutorial-3-eigenvalues-eigenvectors.html
|
||||
Vector3 nv::firstEigenVector(float matrix[6])
|
||||
{
|
||||
// Number of iterations. @@ Use a variable number of iterations.
|
||||
const int NUM = 8;
|
||||
|
||||
Vector3 v(1, 1, 1);
|
||||
for(int i = 0; i < NUM; i++) {
|
||||
float x = v.x() * matrix[0] + v.y() * matrix[1] + v.z() * matrix[2];
|
||||
float y = v.x() * matrix[1] + v.y() * matrix[3] + v.z() * matrix[4];
|
||||
float z = v.x() * matrix[2] + v.y() * matrix[4] + v.z() * matrix[5];
|
||||
|
||||
float norm = max(max(x, y), z);
|
||||
float iv = 1.0f / norm;
|
||||
if (norm == 0.0f) {
|
||||
return Vector3(zero);
|
||||
}
|
||||
|
||||
v.set(x*iv, y*iv, z*iv);
|
||||
}
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
/// Solve eigen system.
|
||||
void Eigen::solve() {
|
||||
semi_definite_symmetric_eigen(matrix, N, eigen_vec, eigen_val);
|
||||
}
|
||||
|
||||
/// Solve eigen system.
|
||||
void Eigen3::solve() {
|
||||
// @@ Use lengyel code that seems to be more optimized.
|
||||
#if 1
|
||||
float v[3*3];
|
||||
semi_definite_symmetric_eigen(matrix, 3, v, eigen_val);
|
||||
|
||||
eigen_vec[0].set(v[0], v[1], v[2]);
|
||||
eigen_vec[1].set(v[3], v[4], v[5]);
|
||||
eigen_vec[2].set(v[6], v[7], v[8]);
|
||||
#else
|
||||
const int maxSweeps = 32;
|
||||
const float epsilon = 1.0e-10f;
|
||||
|
||||
float m11 = matrix[0]; // m(0,0);
|
||||
float m12 = matrix[1]; // m(0,1);
|
||||
float m13 = matrix[2]; // m(0,2);
|
||||
float m22 = matrix[3]; // m(1,1);
|
||||
float m23 = matrix[4]; // m(1,2);
|
||||
float m33 = matrix[5]; // m(2,2);
|
||||
|
||||
//r.SetIdentity();
|
||||
eigen_vec[0].set(1, 0, 0);
|
||||
eigen_vec[1].set(0, 1, 0);
|
||||
eigen_vec[2].set(0, 0, 1);
|
||||
|
||||
for (int a = 0; a < maxSweeps; a++)
|
||||
{
|
||||
// Exit if off-diagonal entries small enough
|
||||
if ((fabs(m12) < epsilon) && (fabs(m13) < epsilon) && (fabs(m23) < epsilon))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// Annihilate (1,2) entry
|
||||
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 * eigen_vec[i].x - s * eigen_vec[i].y;
|
||||
eigen_vec[i].y = s * eigen_vec[i].x + c * eigen_vec[i].y;
|
||||
eigen_vec[i].x = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (1,3) entry
|
||||
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 * eigen_vec[i].x - s * eigen_vec[i].z;
|
||||
eigen_vec[i].z = s * eigen_vec[i].x + c * eigen_vec[i].z;
|
||||
eigen_vec[i].x = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (2,3) entry
|
||||
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 * eigen_vec[i].y - s * eigen_vec[i].z;
|
||||
eigen_vec[i].z = s * eigen_vec[i].y + c * eigen_vec[i].z;
|
||||
eigen_vec[i].y = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
eigen_val[0] = m11;
|
||||
eigen_val[1] = m22;
|
||||
eigen_val[2] = m33;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
Functions
|
||||
---------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
/** @@ I don't remember where did I get this function.
|
||||
* computes the eigen values and eigen vectors
|
||||
* of a semi definite symmetric matrix
|
||||
*
|
||||
* - matrix is stored in column symmetric storage, i.e.
|
||||
* matrix = { m11, m12, m22, m13, m23, m33, m14, m24, m34, m44 ... }
|
||||
* size = n(n+1)/2
|
||||
*
|
||||
* - eigen_vectors (return) = { v1, v2, v3, ..., vn } where vk = vk0, vk1, ..., vkn
|
||||
* size = n^2, must be allocated by caller
|
||||
*
|
||||
* - eigen_values (return) are in decreasing order
|
||||
* size = n, must be allocated by caller
|
||||
*/
|
||||
|
||||
void semi_definite_symmetric_eigen(
|
||||
const float *mat, int n, float *eigen_vec, float *eigen_val
|
||||
) {
|
||||
float *a,*v;
|
||||
float a_norm,a_normEPS,thr,thr_nn;
|
||||
int nb_iter = 0;
|
||||
int jj;
|
||||
int i,j,k,ij,ik,l,m,lm,mq,lq,ll,mm,imv,im,iq,ilv,il,nn;
|
||||
int *index;
|
||||
float a_ij,a_lm,a_ll,a_mm,a_im,a_il;
|
||||
float a_lm_2;
|
||||
float v_ilv,v_imv;
|
||||
float x;
|
||||
float sinx,sinx_2,cosx,cosx_2,sincos;
|
||||
float delta;
|
||||
|
||||
// Number of entries in mat
|
||||
|
||||
nn = (n*(n+1))/2;
|
||||
|
||||
// Step 1: Copy mat to a
|
||||
|
||||
a = new float[nn];
|
||||
|
||||
for( ij=0; ij<nn; ij++ ) {
|
||||
a[ij] = mat[ij];
|
||||
}
|
||||
|
||||
// Ugly Fortran-porting trick: indices for a are between 1 and n
|
||||
a--;
|
||||
|
||||
// Step 2 : Init diagonalization matrix as the unit matrix
|
||||
v = new float[n*n];
|
||||
|
||||
ij = 0;
|
||||
for( i=0; i<n; i++ ) {
|
||||
for( j=0; j<n; j++ ) {
|
||||
if( i==j ) {
|
||||
v[ij++] = 1.0;
|
||||
} else {
|
||||
v[ij++] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Ugly Fortran-porting trick: indices for v are between 1 and n
|
||||
v--;
|
||||
|
||||
// Step 3 : compute the weight of the non diagonal terms
|
||||
ij = 1 ;
|
||||
a_norm = 0.0;
|
||||
for( i=1; i<=n; i++ ) {
|
||||
for( j=1; j<=i; j++ ) {
|
||||
if( i!=j ) {
|
||||
a_ij = a[ij];
|
||||
a_norm += a_ij*a_ij;
|
||||
}
|
||||
ij++;
|
||||
}
|
||||
}
|
||||
|
||||
if( a_norm != 0.0 ) {
|
||||
|
||||
a_normEPS = a_norm*EPS;
|
||||
thr = a_norm ;
|
||||
|
||||
// Step 4 : rotations
|
||||
while( thr > a_normEPS && nb_iter < MAX_ITER ) {
|
||||
|
||||
nb_iter++;
|
||||
thr_nn = thr / nn;
|
||||
|
||||
for( l=1 ; l< n; l++ ) {
|
||||
for( m=l+1; m<=n; m++ ) {
|
||||
|
||||
// compute sinx and cosx
|
||||
|
||||
lq = (l*l-l)/2;
|
||||
mq = (m*m-m)/2;
|
||||
|
||||
lm = l+mq;
|
||||
a_lm = a[lm];
|
||||
a_lm_2 = a_lm*a_lm;
|
||||
|
||||
if( a_lm_2 < thr_nn ) {
|
||||
continue ;
|
||||
}
|
||||
|
||||
ll = l+lq;
|
||||
mm = m+mq;
|
||||
a_ll = a[ll];
|
||||
a_mm = a[mm];
|
||||
|
||||
delta = a_ll - a_mm;
|
||||
|
||||
if( delta == 0.0f ) {
|
||||
x = - PI/4 ;
|
||||
} else {
|
||||
x = - atanf( (a_lm+a_lm) / delta ) / 2.0f ;
|
||||
}
|
||||
|
||||
sinx = sinf(x);
|
||||
cosx = cosf(x);
|
||||
sinx_2 = sinx*sinx;
|
||||
cosx_2 = cosx*cosx;
|
||||
sincos = sinx*cosx;
|
||||
|
||||
// rotate L and M columns
|
||||
|
||||
ilv = n*(l-1);
|
||||
imv = n*(m-1);
|
||||
|
||||
for( i=1; i<=n;i++ ) {
|
||||
if( (i!=l) && (i!=m) ) {
|
||||
iq = (i*i-i)/2;
|
||||
|
||||
if( i<m ) {
|
||||
im = i + mq;
|
||||
} else {
|
||||
im = m + iq;
|
||||
}
|
||||
a_im = a[im];
|
||||
|
||||
if( i<l ) {
|
||||
il = i + lq;
|
||||
} else {
|
||||
il = l + iq;
|
||||
}
|
||||
a_il = a[il];
|
||||
|
||||
a[il] = a_il*cosx - a_im*sinx;
|
||||
a[im] = a_il*sinx + a_im*cosx;
|
||||
}
|
||||
|
||||
ilv++;
|
||||
imv++;
|
||||
|
||||
v_ilv = v[ilv];
|
||||
v_imv = v[imv];
|
||||
|
||||
v[ilv] = cosx*v_ilv - sinx*v_imv;
|
||||
v[imv] = sinx*v_ilv + cosx*v_imv;
|
||||
}
|
||||
|
||||
x = a_lm*sincos; x+=x;
|
||||
|
||||
a[ll] = a_ll*cosx_2 + a_mm*sinx_2 - x;
|
||||
a[mm] = a_ll*sinx_2 + a_mm*cosx_2 + x;
|
||||
a[lm] = 0.0;
|
||||
|
||||
thr = fabs( thr - a_lm_2 );
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Step 5: index conversion and copy eigen values
|
||||
|
||||
// back from Fortran to C++
|
||||
a++;
|
||||
|
||||
for( i=0; i<n; i++ ) {
|
||||
k = i + (i*(i+1))/2;
|
||||
eigen_val[i] = a[k];
|
||||
}
|
||||
|
||||
delete[] a;
|
||||
|
||||
// Step 6: sort the eigen values and eigen vectors
|
||||
|
||||
index = new int[n];
|
||||
for( i=0; i<n; i++ ) {
|
||||
index[i] = i;
|
||||
}
|
||||
|
||||
for( i=0; i<(n-1); i++ ) {
|
||||
x = eigen_val[i];
|
||||
k = i;
|
||||
|
||||
for( j=i+1; j<n; j++ ) {
|
||||
if( x < eigen_val[j] ) {
|
||||
k = j;
|
||||
x = eigen_val[j];
|
||||
}
|
||||
}
|
||||
|
||||
eigen_val[k] = eigen_val[i];
|
||||
eigen_val[i] = x;
|
||||
|
||||
jj = index[k];
|
||||
index[k] = index[i];
|
||||
index[i] = jj;
|
||||
}
|
||||
|
||||
|
||||
// Step 7: save the eigen vectors
|
||||
|
||||
v++; // back from Fortran to to C++
|
||||
|
||||
ij = 0;
|
||||
for( k=0; k<n; k++ ) {
|
||||
ik = index[k]*n;
|
||||
for( i=0; i<n; i++ ) {
|
||||
eigen_vec[ij++] = v[ik++];
|
||||
}
|
||||
}
|
||||
|
||||
delete[] v ;
|
||||
delete[] index;
|
||||
return;
|
||||
}
|
||||
|
||||
//_________________________________________________________
|
||||
|
||||
|
||||
// Eric Lengyel code:
|
||||
// http://www.terathon.com/code/linear.html
|
||||
#if 0
|
||||
|
||||
const float epsilon = 1.0e-10F;
|
||||
const int maxSweeps = 32;
|
||||
|
||||
|
||||
struct Matrix3D
|
||||
{
|
||||
float n[3][3];
|
||||
|
||||
float& operator()(int i, int j)
|
||||
{
|
||||
return (n[j][i]);
|
||||
}
|
||||
|
||||
const float& operator()(int i, int j) const
|
||||
{
|
||||
return (n[j][i]);
|
||||
}
|
||||
|
||||
void SetIdentity(void)
|
||||
{
|
||||
n[0][0] = n[1][1] = n[2][2] = 1.0F;
|
||||
n[0][1] = n[0][2] = n[1][0] = n[1][2] = n[2][0] = n[2][1] = 0.0F;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
void CalculateEigensystem(const Matrix3D& m, float *lambda, Matrix3D& r)
|
||||
{
|
||||
float m11 = m(0,0);
|
||||
float m12 = m(0,1);
|
||||
float m13 = m(0,2);
|
||||
float m22 = m(1,1);
|
||||
float m23 = m(1,2);
|
||||
float m33 = m(2,2);
|
||||
|
||||
r.SetIdentity();
|
||||
for (int a = 0; a < maxSweeps; a++)
|
||||
{
|
||||
// Exit if off-diagonal entries small enough
|
||||
if ((Fabs(m12) < epsilon) && (Fabs(m13) < epsilon) &&
|
||||
(Fabs(m23) < epsilon)) break;
|
||||
|
||||
// Annihilate (1,2) entry
|
||||
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(i,0) - s * r(i,1);
|
||||
r(i,1) = s * r(i,0) + c * r(i,1);
|
||||
r(i,0) = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (1,3) entry
|
||||
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(i,0) - s * r(i,2);
|
||||
r(i,2) = s * r(i,0) + c * r(i,2);
|
||||
r(i,0) = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (2,3) entry
|
||||
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(i,1) - s * r(i,2);
|
||||
r(i,2) = s * r(i,1) + c * r(i,2);
|
||||
r(i,1) = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
lambda[0] = m11;
|
||||
lambda[1] = m22;
|
||||
lambda[2] = m33;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
@ -1,140 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_EIGEN_H
|
||||
#define NV_MATH_EIGEN_H
|
||||
|
||||
#include <nvcore/Containers.h> // swap
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
// Compute first eigen vector using the power method.
|
||||
Vector3 firstEigenVector(float matrix[6]);
|
||||
|
||||
/// Generic eigen-solver.
|
||||
class Eigen
|
||||
{
|
||||
public:
|
||||
|
||||
/// Ctor.
|
||||
Eigen(uint n) : N(n)
|
||||
{
|
||||
uint size = n * (n + 1) / 2;
|
||||
matrix = new float[size];
|
||||
eigen_vec = new float[N*N];
|
||||
eigen_val = new float[N];
|
||||
}
|
||||
|
||||
/// Dtor.
|
||||
~Eigen()
|
||||
{
|
||||
delete [] matrix;
|
||||
delete [] eigen_vec;
|
||||
delete [] eigen_val;
|
||||
}
|
||||
|
||||
NVMATH_API void solve();
|
||||
|
||||
/// Matrix accesor.
|
||||
float & operator()(uint x, uint y)
|
||||
{
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Matrix const accessor.
|
||||
float operator()(uint x, uint y) const
|
||||
{
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
Vector3 eigenVector3(uint i) const
|
||||
{
|
||||
nvCheck(3 == N);
|
||||
nvCheck(i < N);
|
||||
return Vector3(eigen_vec[i*N+0], eigen_vec[i*N+1], eigen_vec[i*N+2]);
|
||||
}
|
||||
|
||||
Vector4 eigenVector4(uint i) const
|
||||
{
|
||||
nvCheck(4 == N);
|
||||
nvCheck(i < N);
|
||||
return Vector4(eigen_vec[i*N+0], eigen_vec[i*N+1], eigen_vec[i*N+2], eigen_vec[i*N+3]);
|
||||
}
|
||||
|
||||
float eigenValue(uint i) const
|
||||
{
|
||||
nvCheck(i < N);
|
||||
return eigen_val[i];
|
||||
}
|
||||
|
||||
private:
|
||||
const uint N;
|
||||
float * matrix;
|
||||
float * eigen_vec;
|
||||
float * eigen_val;
|
||||
};
|
||||
|
||||
|
||||
/// 3x3 eigen-solver.
|
||||
/// Based on Eric Lengyel's code:
|
||||
/// http://www.terathon.com/code/linear.html
|
||||
class Eigen3
|
||||
{
|
||||
public:
|
||||
|
||||
/** Ctor. */
|
||||
Eigen3() {}
|
||||
|
||||
NVMATH_API void solve();
|
||||
|
||||
/// Matrix accesor.
|
||||
float & operator()(uint x, uint y)
|
||||
{
|
||||
nvDebugCheck( x < 3 && y < 3 );
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Matrix const accessor.
|
||||
float operator()(uint x, uint y) const
|
||||
{
|
||||
nvDebugCheck( x < 3 && y < 3 );
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Get ith eigen vector.
|
||||
Vector3 eigenVector(uint i) const
|
||||
{
|
||||
nvCheck(i < 3);
|
||||
return eigen_vec[i];
|
||||
}
|
||||
|
||||
/** Get ith eigen value. */
|
||||
float eigenValue(uint i) const
|
||||
{
|
||||
nvCheck(i < 3);
|
||||
return eigen_val[i];
|
||||
}
|
||||
|
||||
private:
|
||||
float matrix[3+2+1];
|
||||
Vector3 eigen_vec[3];
|
||||
float eigen_val[3];
|
||||
};
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_EIGEN_H
|
||||
@ -1,134 +0,0 @@
|
||||
// License: Wild Magic License Version 3
|
||||
// http://geometrictools.com/License/WildMagic3License.pdf
|
||||
|
||||
#include "Fitting.h"
|
||||
#include "Eigen.h"
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
/** Fit a 3d line to the given set of points.
|
||||
*
|
||||
* Based on code from:
|
||||
* http://geometrictools.com/
|
||||
*/
|
||||
Line3 Fit::bestLine(const Array<Vector3> & pointArray)
|
||||
{
|
||||
nvDebugCheck(pointArray.count() > 0);
|
||||
|
||||
Line3 line;
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
const float inv_num = 1.0f / pointCount;
|
||||
|
||||
// compute the mean of the points
|
||||
Vector3 center(zero);
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
center += pointArray[i];
|
||||
}
|
||||
line.setOrigin(center * inv_num);
|
||||
|
||||
// compute the covariance matrix of the points
|
||||
float covariance[6] = {0, 0, 0, 0, 0, 0};
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
Vector3 diff = pointArray[i] - line.origin();
|
||||
covariance[0] += diff.x() * diff.x();
|
||||
covariance[1] += diff.x() * diff.y();
|
||||
covariance[2] += diff.x() * diff.z();
|
||||
covariance[3] += diff.y() * diff.y();
|
||||
covariance[4] += diff.y() * diff.z();
|
||||
covariance[5] += diff.z() * diff.z();
|
||||
}
|
||||
|
||||
line.setDirection(normalizeSafe(firstEigenVector(covariance), Vector3(zero), 0.0f));
|
||||
|
||||
// @@ This variant is from David Eberly... I'm not sure how that works.
|
||||
/*sum_xx *= inv_num;
|
||||
sum_xy *= inv_num;
|
||||
sum_xz *= inv_num;
|
||||
sum_yy *= inv_num;
|
||||
sum_yz *= inv_num;
|
||||
sum_zz *= inv_num;
|
||||
|
||||
// set up the eigensolver
|
||||
Eigen3 ES;
|
||||
ES(0,0) = sum_yy + sum_zz;
|
||||
ES(0,1) = -sum_xy;
|
||||
ES(0,2) = -sum_xz;
|
||||
ES(1,1) = sum_xx + sum_zz;
|
||||
ES(1,2) = -sum_yz;
|
||||
ES(2,2) = sum_xx + sum_yy;
|
||||
|
||||
// compute eigenstuff, smallest eigenvalue is in last position
|
||||
ES.solve();
|
||||
|
||||
line.setDirection(ES.eigenVector(2));
|
||||
|
||||
nvCheck( isNormalized(line.direction()) );
|
||||
*/
|
||||
return line;
|
||||
}
|
||||
|
||||
|
||||
/** Fit a 3d plane to the given set of points.
|
||||
*
|
||||
* Based on code from:
|
||||
* http://geometrictools.com/
|
||||
*/
|
||||
Vector4 Fit::bestPlane(const Array<Vector3> & pointArray)
|
||||
{
|
||||
Vector3 center(zero);
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
const float inv_num = 1.0f / pointCount;
|
||||
|
||||
// compute the mean of the points
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
center += pointArray[i];
|
||||
}
|
||||
center *= inv_num;
|
||||
|
||||
// compute the covariance matrix of the points
|
||||
float sum_xx = 0.0f;
|
||||
float sum_xy = 0.0f;
|
||||
float sum_xz = 0.0f;
|
||||
float sum_yy = 0.0f;
|
||||
float sum_yz = 0.0f;
|
||||
float sum_zz = 0.0f;
|
||||
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
Vector3 diff = pointArray[i] - center;
|
||||
sum_xx += diff.x() * diff.x();
|
||||
sum_xy += diff.x() * diff.y();
|
||||
sum_xz += diff.x() * diff.z();
|
||||
sum_yy += diff.y() * diff.y();
|
||||
sum_yz += diff.y() * diff.z();
|
||||
sum_zz += diff.z() * diff.z();
|
||||
}
|
||||
|
||||
sum_xx *= inv_num;
|
||||
sum_xy *= inv_num;
|
||||
sum_xz *= inv_num;
|
||||
sum_yy *= inv_num;
|
||||
sum_yz *= inv_num;
|
||||
sum_zz *= inv_num;
|
||||
|
||||
// set up the eigensolver
|
||||
Eigen3 ES;
|
||||
ES(0,0) = sum_yy + sum_zz;
|
||||
ES(0,1) = -sum_xy;
|
||||
ES(0,2) = -sum_xz;
|
||||
ES(1,1) = sum_xx + sum_zz;
|
||||
ES(1,2) = -sum_yz;
|
||||
ES(2,2) = sum_xx + sum_yy;
|
||||
|
||||
// compute eigenstuff, greatest eigenvalue is in first position
|
||||
ES.solve();
|
||||
|
||||
Vector3 normal = ES.eigenVector(0);
|
||||
nvCheck(isNormalized(normal));
|
||||
|
||||
float offset = dot(normal, center);
|
||||
|
||||
return Vector4(normal, offset);
|
||||
}
|
||||
@ -1,78 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_FITTING_H
|
||||
#define NV_MATH_FITTING_H
|
||||
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// 3D Line.
|
||||
struct Line3
|
||||
{
|
||||
/// Ctor.
|
||||
Line3() : m_origin(zero), m_direction(zero)
|
||||
{
|
||||
}
|
||||
|
||||
/// Copy ctor.
|
||||
Line3(const Line3 & l) : m_origin(l.m_origin), m_direction(l.m_direction)
|
||||
{
|
||||
}
|
||||
|
||||
/// Ctor.
|
||||
Line3(Vector3::Arg o, Vector3::Arg d) : m_origin(o), m_direction(d)
|
||||
{
|
||||
}
|
||||
|
||||
/// Normalize the line.
|
||||
void normalize()
|
||||
{
|
||||
m_direction = nv::normalize(m_direction);
|
||||
}
|
||||
|
||||
/// Project a point onto the line.
|
||||
Vector3 projectPoint(Vector3::Arg point) const
|
||||
{
|
||||
nvDebugCheck(isNormalized(m_direction));
|
||||
|
||||
Vector3 v = point - m_origin;
|
||||
return m_origin + m_direction * dot(m_direction, v);
|
||||
}
|
||||
|
||||
/// Compute distance to line.
|
||||
float distanceToPoint(Vector3::Arg point) const
|
||||
{
|
||||
nvDebugCheck(isNormalized(m_direction));
|
||||
|
||||
Vector3 v = point - m_origin;
|
||||
Vector3 l = v - m_direction * dot(m_direction, v);
|
||||
|
||||
return length(l);
|
||||
}
|
||||
|
||||
const Vector3 & origin() const { return m_origin; }
|
||||
void setOrigin(Vector3::Arg value) { m_origin = value; }
|
||||
|
||||
const Vector3 & direction() const { return m_direction; }
|
||||
void setDirection(Vector3::Arg value) { m_direction = value; }
|
||||
|
||||
|
||||
private:
|
||||
Vector3 m_origin;
|
||||
Vector3 m_direction;
|
||||
};
|
||||
|
||||
|
||||
namespace Fit
|
||||
{
|
||||
|
||||
NVMATH_API Line3 bestLine(const Array<Vector3> & pointArray);
|
||||
NVMATH_API Vector4 bestPlane(const Array<Vector3> & pointArray);
|
||||
|
||||
} // Fit namespace
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // _PI_MATHLIB_FITTING_H_
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue