eigensolver for symmetric Mat3
This commit is contained in:
parent
4e08955ed5
commit
30f4d29d82
32
t.cc
32
t.cc
@ -2295,7 +2295,7 @@ cout <<"non-zero = "<< ((n&1)?p.odd_powers():p.even_powers())<<std::endl;
|
|||||||
cout <<"value = "<<value(p,1.23456789)<<" "<<odd_value(p,1.23456789)+even_value(p,1.23456789)<<endl;
|
cout <<"value = "<<value(p,1.23456789)<<" "<<odd_value(p,1.23456789)+even_value(p,1.23456789)<<endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(1)
|
if(0)
|
||||||
{
|
{
|
||||||
NRVec<int> v({1,2,3,4});
|
NRVec<int> v({1,2,3,4});
|
||||||
cout <<v;
|
cout <<v;
|
||||||
@ -2321,5 +2321,35 @@ Polynomial<double> pp({1,2,3,4,5});
|
|||||||
cout<<pp;
|
cout<<pp;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if(1)
|
||||||
|
{
|
||||||
|
//prepare random symmetric mat3
|
||||||
|
int seed;
|
||||||
|
int f=open("/dev/random",O_RDONLY);
|
||||||
|
if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random");
|
||||||
|
close(f);
|
||||||
|
srand(seed);
|
||||||
|
|
||||||
|
NRMat<double> tmp(3,3);
|
||||||
|
tmp.randomize(2.);
|
||||||
|
Mat3<double> mm(tmp);
|
||||||
|
mm.symmetrize();
|
||||||
|
NRMat<double> m(&mm[0][0],3,3);
|
||||||
|
cout <<m<<"3 3\n"<<mm<<endl;
|
||||||
|
|
||||||
|
NRVec<double> w(3);
|
||||||
|
diagonalize(m,w);
|
||||||
|
cout << w<<m;
|
||||||
|
|
||||||
|
Vec3<double> ww;
|
||||||
|
Mat3<double> vv;
|
||||||
|
mm.eivec_sym(ww,vv);
|
||||||
|
cout <<"3\n"<<ww<<"\n3 3\n"<<vv<<endl;
|
||||||
|
|
||||||
|
NRVec<double> www(&ww[0],3);
|
||||||
|
NRMat<double> vvv(&vv[0][0],3,3);
|
||||||
|
cout<<"eival error = "<<(w-www).norm()<<endl;
|
||||||
|
cout<<"eivec error = "<<(m.diffabs(vvv)).norm()<<endl; //just ignore signs due to arb. phases (not full check)
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
297
vecmat3.cc
297
vecmat3.cc
@ -127,6 +127,7 @@ const Vec3<T> Mat3<T>::operator*(const Vec3<T> &rhs) const
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//cf. https://en.wikipedia.org/wiki/Euler_angles and NASA paper cited therein
|
//cf. https://en.wikipedia.org/wiki/Euler_angles and NASA paper cited therein
|
||||||
template<typename T>
|
template<typename T>
|
||||||
void euler2rotmat(const T *eul, Mat3<T> &a, const char *type, bool transpose, bool direction, bool reverse)
|
void euler2rotmat(const T *eul, Mat3<T> &a, const char *type, bool transpose, bool direction, bool reverse)
|
||||||
@ -486,7 +487,303 @@ return s;
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
void Mat3<T>::symmetrize()
|
||||||
|
{
|
||||||
|
T tmp=(q[0][1]+q[1][0])/2;
|
||||||
|
q[0][1]=q[1][0]=tmp;
|
||||||
|
tmp=(q[0][2]+q[2][0])/2;
|
||||||
|
q[0][2]=q[2][0]=tmp;
|
||||||
|
tmp=(q[2][1]+q[1][2])/2;
|
||||||
|
q[2][1]=q[1][2]=tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
//eigensolver for 3x3 matrix by Joachim Kopp - analytic formula version,
|
||||||
|
//might be unstable for ill-conditioned ones, then use other methods
|
||||||
|
//cf. arxiv physics 0610206v3
|
||||||
|
//
|
||||||
|
//// Numerical diagonalization of 3x3 matrcies
|
||||||
|
//// Copyright (C) 2006 Joachim Kopp
|
||||||
|
//// ----------------------------------------------------------------------------
|
||||||
|
//// This library is free software; you can redistribute it and/or
|
||||||
|
//// modify it under the terms of the GNU Lesser General Public
|
||||||
|
//// License as published by the Free Software Foundation; either
|
||||||
|
//// version 2.1 of the License, or (at your option) any later version.
|
||||||
|
////
|
||||||
|
//// This library is distributed in the hope that it will be useful,
|
||||||
|
//// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
//// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||||
|
//// Lesser General Public License for more details.
|
||||||
|
////
|
||||||
|
//// You should have received a copy of the GNU Lesser General Public
|
||||||
|
//// License along with this library; if not, write to the Free Software
|
||||||
|
//// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||||
|
//// ----------------------------------------------------------------------------
|
||||||
|
//
|
||||||
|
|
||||||
|
//numeric_limits not available on some crosscompilers for small MCUs
|
||||||
|
#ifdef ARM_SOURCE37
|
||||||
|
#define DBL_EPSILON 1.19209290e-07f
|
||||||
|
#else
|
||||||
|
#define DBL_EPSILON std::numeric_limits<T>::epsilon()
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define M_SQRT3 1.73205080756887729352744634151 // sqrt(3)
|
||||||
|
#define SQR(x) ((x)*(x)) // x^2
|
||||||
|
//
|
||||||
|
template <typename T>
|
||||||
|
void Mat3<T>::eival_sym(Vec3<T> &w) const
|
||||||
|
{
|
||||||
|
T m, c1, c0;
|
||||||
|
|
||||||
|
// Determine coefficients of characteristic poynomial. We write
|
||||||
|
// | a d f |
|
||||||
|
// A = | d* b e |
|
||||||
|
// | f* e* c |
|
||||||
|
T de = q[0][1] * q[1][2]; // d * e
|
||||||
|
T dd = SQR(q[0][1]); // d^2
|
||||||
|
T ee = SQR(q[1][2]); // e^2
|
||||||
|
T ff = SQR(q[0][2]); // f^2
|
||||||
|
m = q[0][0] + q[1][1] + q[2][2];
|
||||||
|
c1 = (q[0][0]*q[1][1] + q[0][0]*q[2][2] + q[1][1]*q[2][2]) // a*b + a*c + b*c - d^2 - e^2 - f^2
|
||||||
|
- (dd + ee + ff);
|
||||||
|
c0 = q[2][2]*dd + q[0][0]*ee + q[1][1]*ff - q[0][0]*q[1][1]*q[2][2]
|
||||||
|
- 2.0 * q[0][2]*de; // c*d^2 + a*e^2 + b*f^2 - a*b*c - 2*f*d*e)
|
||||||
|
|
||||||
|
T p, sqrt_p, q, c, s, phi;
|
||||||
|
p = SQR(m) - 3.0*c1;
|
||||||
|
q = m*(p - (3.0/2.0)*c1) - (27.0/2.0)*c0;
|
||||||
|
sqrt_p = sqrt(abs(p));
|
||||||
|
|
||||||
|
phi = 27.0 * ( 0.25*SQR(c1)*(p - c1) + c0*(q + 27.0/4.0*c0));
|
||||||
|
phi = (1.0/3.0) * atan2(sqrt(abs(phi)), q);
|
||||||
|
|
||||||
|
c = sqrt_p*cos(phi);
|
||||||
|
s = (1.0/M_SQRT3)*sqrt_p*sin(phi);
|
||||||
|
|
||||||
|
w[1] = (1.0/3.0)*(m - c);
|
||||||
|
w[2] = w[1] + s;
|
||||||
|
w[0] = w[1] + c;
|
||||||
|
w[1] -= s;
|
||||||
|
|
||||||
|
//sort in ascending order
|
||||||
|
if(w[0]>w[1]) {T tmp=w[0]; w[0]=w[1]; w[1]=tmp;}
|
||||||
|
if(w[0]>w[2]) {T tmp=w[0]; w[0]=w[2]; w[2]=tmp;}
|
||||||
|
if(w[1]>w[2]) {T tmp=w[1]; w[1]=w[2]; w[2]=tmp;}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculates the eigenvalues and normalized eigenvectors of a symmetric 3x3
|
||||||
|
// matrix A using Cardano's method for the eigenvalues and an analytical
|
||||||
|
// method based on vector cross products for the eigenvectors.
|
||||||
|
// Only the diagonal and upper triangular parts of A need to contain meaningful
|
||||||
|
// values. However, all of A may be used as temporary storage and may hence be
|
||||||
|
// destroyed.
|
||||||
|
// ----------------------------------------------------------------------------
|
||||||
|
// Parameters:
|
||||||
|
// A: The symmetric input matrix
|
||||||
|
// Q: Storage buffer for eigenvectors
|
||||||
|
// w: Storage buffer for eigenvalues
|
||||||
|
// ----------------------------------------------------------------------------
|
||||||
|
// Return value:
|
||||||
|
// 0: Success
|
||||||
|
// -1: Error
|
||||||
|
// ----------------------------------------------------------------------------
|
||||||
|
// Dependencies:
|
||||||
|
// dsyevc3()
|
||||||
|
// ----------------------------------------------------------------------------
|
||||||
|
// Version history:
|
||||||
|
// v1.1 (12 Mar 2012): Removed access to lower triangualr part of A
|
||||||
|
// (according to the documentation, only the upper triangular part needs
|
||||||
|
// to be filled)
|
||||||
|
// v1.0: First released version
|
||||||
|
// ----------------------------------------------------------------------------
|
||||||
|
template <typename T>
|
||||||
|
void Mat3<T>::eivec_sym(Vec3<T> &w, Mat3 &v) const
|
||||||
|
{
|
||||||
|
T norm; // Squared norm or inverse norm of current eigenvector
|
||||||
|
T n0, n1; // Norm of first and second columns of A
|
||||||
|
T n0tmp, n1tmp; // "Templates" for the calculation of n0/n1 - saves a few FLOPS
|
||||||
|
T thresh; // Small number used as threshold for floating point comparisons
|
||||||
|
T error; // Estimated maximum roundoff error in some steps
|
||||||
|
T wmax; // The eigenvalue of maximum modulus
|
||||||
|
T f, t; // Intermediate storage
|
||||||
|
int i, j; // Loop counters
|
||||||
|
|
||||||
|
// Calculate eigenvalues
|
||||||
|
eival_sym(w);
|
||||||
|
Mat3<T> A(*this); //scratch copy
|
||||||
|
|
||||||
|
wmax = fabs(w[0]);
|
||||||
|
if ((t=fabs(w[1])) > wmax)
|
||||||
|
wmax = t;
|
||||||
|
if ((t=fabs(w[2])) > wmax)
|
||||||
|
wmax = t;
|
||||||
|
thresh = SQR(8.0 * DBL_EPSILON * wmax);
|
||||||
|
|
||||||
|
// Prepare calculation of eigenvectors
|
||||||
|
n0tmp = SQR(A[0][1]) + SQR(A[0][2]);
|
||||||
|
n1tmp = SQR(A[0][1]) + SQR(A[1][2]);
|
||||||
|
v[0][1] = A[0][1]*A[1][2] - A[0][2]*A[1][1];
|
||||||
|
v[1][1] = A[0][2]*A[0][1] - A[1][2]*A[0][0];
|
||||||
|
v[2][1] = SQR(A[0][1]);
|
||||||
|
|
||||||
|
// Calculate first eigenvector by the formula
|
||||||
|
// v[0] = (A - w[0]).e1 x (A - w[0]).e2
|
||||||
|
A[0][0] -= w[0];
|
||||||
|
A[1][1] -= w[0];
|
||||||
|
v[0][0] = v[0][1] + A[0][2]*w[0];
|
||||||
|
v[1][0] = v[1][1] + A[1][2]*w[0];
|
||||||
|
v[2][0] = A[0][0]*A[1][1] - v[2][1];
|
||||||
|
norm = SQR(v[0][0]) + SQR(v[1][0]) + SQR(v[2][0]);
|
||||||
|
n0 = n0tmp + SQR(A[0][0]);
|
||||||
|
n1 = n1tmp + SQR(A[1][1]);
|
||||||
|
error = n0 * n1;
|
||||||
|
|
||||||
|
if (n0 <= thresh) // If the first column is zero, then (1,0,0) is an eigenvector
|
||||||
|
{
|
||||||
|
v[0][0] = 1.0;
|
||||||
|
v[1][0] = 0.0;
|
||||||
|
v[2][0] = 0.0;
|
||||||
|
}
|
||||||
|
else if (n1 <= thresh) // If the second column is zero, then (0,1,0) is an eigenvector
|
||||||
|
{
|
||||||
|
v[0][0] = 0.0;
|
||||||
|
v[1][0] = 1.0;
|
||||||
|
v[2][0] = 0.0;
|
||||||
|
}
|
||||||
|
else if (norm < SQR(64.0 * DBL_EPSILON) * error)
|
||||||
|
{ // If angle between A[0] and A[1] is too small, don't use
|
||||||
|
t = SQR(A[0][1]); // cross product, but calculate v ~ (1, -A0/A1, 0)
|
||||||
|
f = -A[0][0] / A[0][1];
|
||||||
|
if (SQR(A[1][1]) > t)
|
||||||
|
{
|
||||||
|
t = SQR(A[1][1]);
|
||||||
|
f = -A[0][1] / A[1][1];
|
||||||
|
}
|
||||||
|
if (SQR(A[1][2]) > t)
|
||||||
|
f = -A[0][2] / A[1][2];
|
||||||
|
norm = 1.0/sqrt(1 + SQR(f));
|
||||||
|
v[0][0] = norm;
|
||||||
|
v[1][0] = f * norm;
|
||||||
|
v[2][0] = 0.0;
|
||||||
|
}
|
||||||
|
else // This is the standard branch
|
||||||
|
{
|
||||||
|
norm = sqrt(1.0 / norm);
|
||||||
|
for (j=0; j < 3; j++)
|
||||||
|
v[j][0] = v[j][0] * norm;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Prepare calculation of second eigenvector
|
||||||
|
t = w[0] - w[1];
|
||||||
|
if (fabs(t) > 8.0 * DBL_EPSILON * wmax)
|
||||||
|
{
|
||||||
|
// For non-degenerate eigenvalue, calculate second eigenvector by the formula
|
||||||
|
// v[1] = (A - w[1]).e1 x (A - w[1]).e2
|
||||||
|
A[0][0] += t;
|
||||||
|
A[1][1] += t;
|
||||||
|
v[0][1] = v[0][1] + A[0][2]*w[1];
|
||||||
|
v[1][1] = v[1][1] + A[1][2]*w[1];
|
||||||
|
v[2][1] = A[0][0]*A[1][1] - v[2][1];
|
||||||
|
norm = SQR(v[0][1]) + SQR(v[1][1]) + SQR(v[2][1]);
|
||||||
|
n0 = n0tmp + SQR(A[0][0]);
|
||||||
|
n1 = n1tmp + SQR(A[1][1]);
|
||||||
|
error = n0 * n1;
|
||||||
|
|
||||||
|
if (n0 <= thresh) // If the first column is zero, then (1,0,0) is an eigenvector
|
||||||
|
{
|
||||||
|
v[0][1] = 1.0;
|
||||||
|
v[1][1] = 0.0;
|
||||||
|
v[2][1] = 0.0;
|
||||||
|
}
|
||||||
|
else if (n1 <= thresh) // If the second column is zero, then (0,1,0) is an eigenvector
|
||||||
|
{
|
||||||
|
v[0][1] = 0.0;
|
||||||
|
v[1][1] = 1.0;
|
||||||
|
v[2][1] = 0.0;
|
||||||
|
}
|
||||||
|
else if (norm < SQR(64.0 * DBL_EPSILON) * error)
|
||||||
|
{ // If angle between A[0] and A[1] is too small, don't use
|
||||||
|
t = SQR(A[0][1]); // cross product, but calculate v ~ (1, -A0/A1, 0)
|
||||||
|
f = -A[0][0] / A[0][1];
|
||||||
|
if (SQR(A[1][1]) > t)
|
||||||
|
{
|
||||||
|
t = SQR(A[1][1]);
|
||||||
|
f = -A[0][1] / A[1][1];
|
||||||
|
}
|
||||||
|
if (SQR(A[1][2]) > t)
|
||||||
|
f = -A[0][2] / A[1][2];
|
||||||
|
norm = 1.0/sqrt(1 + SQR(f));
|
||||||
|
v[0][1] = norm;
|
||||||
|
v[1][1] = f * norm;
|
||||||
|
v[2][1] = 0.0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
norm = sqrt(1.0 / norm);
|
||||||
|
for (j=0; j < 3; j++)
|
||||||
|
v[j][1] = v[j][1] * norm;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// For degenerate eigenvalue, calculate second eigenvector according to
|
||||||
|
// v[1] = v[0] x (A - w[1]).e[i]
|
||||||
|
//
|
||||||
|
// This would really get to complicated if we could not assume all of A to
|
||||||
|
// contain meaningful values.
|
||||||
|
A[1][0] = A[0][1];
|
||||||
|
A[2][0] = A[0][2];
|
||||||
|
A[2][1] = A[1][2];
|
||||||
|
A[0][0] += w[0];
|
||||||
|
A[1][1] += w[0];
|
||||||
|
for (i=0; i < 3; i++)
|
||||||
|
{
|
||||||
|
A[i][i] -= w[1];
|
||||||
|
n0 = SQR(A[0][i]) + SQR(A[1][i]) + SQR(A[2][i]);
|
||||||
|
if (n0 > thresh)
|
||||||
|
{
|
||||||
|
v[0][1] = v[1][0]*A[2][i] - v[2][0]*A[1][i];
|
||||||
|
v[1][1] = v[2][0]*A[0][i] - v[0][0]*A[2][i];
|
||||||
|
v[2][1] = v[0][0]*A[1][i] - v[1][0]*A[0][i];
|
||||||
|
norm = SQR(v[0][1]) + SQR(v[1][1]) + SQR(v[2][1]);
|
||||||
|
if (norm > SQR(256.0 * DBL_EPSILON) * n0) // Accept cross product only if the angle between
|
||||||
|
{ // the two vectors was not too small
|
||||||
|
norm = sqrt(1.0 / norm);
|
||||||
|
for (j=0; j < 3; j++)
|
||||||
|
v[j][1] = v[j][1] * norm;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (i == 3) // This means that any vector orthogonal to v[0] is an EV.
|
||||||
|
{
|
||||||
|
for (j=0; j < 3; j++)
|
||||||
|
if (v[j][0] != 0.0) // Find nonzero element of v[0] ...
|
||||||
|
{ // ... and swap it with the next one
|
||||||
|
norm = 1.0 / sqrt(SQR(v[j][0]) + SQR(v[(j+1)%3][0]));
|
||||||
|
v[j][1] = v[(j+1)%3][0] * norm;
|
||||||
|
v[(j+1)%3][1] = -v[j][0] * norm;
|
||||||
|
v[(j+2)%3][1] = 0.0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Calculate third eigenvector according to
|
||||||
|
// v[2] = v[0] x v[1]
|
||||||
|
v[0][2] = v[1][0]*v[2][1] - v[2][0]*v[1][1];
|
||||||
|
v[1][2] = v[2][0]*v[0][1] - v[0][0]*v[2][1];
|
||||||
|
v[2][2] = v[0][0]*v[1][1] - v[1][0]*v[0][1];
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#undef SQR
|
||||||
|
//end eigensolver for 3x3 matrix
|
||||||
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
//force instantization
|
//force instantization
|
||||||
#define INSTANTIZE(T) \
|
#define INSTANTIZE(T) \
|
||||||
|
|||||||
@ -145,7 +145,9 @@ public:
|
|||||||
//C-style IO
|
//C-style IO
|
||||||
int fprintf(FILE *f, const char *format) const {int n= ::fprintf(f,format,q[0][0],q[0][1],q[0][2]); n+=::fprintf(f,format,q[1][0],q[1][1],q[1][2]); n+=::fprintf(f,format,q[2][0],q[2][1],q[2][2]); return n;};
|
int fprintf(FILE *f, const char *format) const {int n= ::fprintf(f,format,q[0][0],q[0][1],q[0][2]); n+=::fprintf(f,format,q[1][0],q[1][1],q[1][2]); n+=::fprintf(f,format,q[2][0],q[2][1],q[2][2]); return n;};
|
||||||
int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);};
|
int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);};
|
||||||
|
void symmetrize(); //average offdiagonal elements
|
||||||
|
void eival_sym(Vec3<T> &w) const; //only for real symmetric matrix, symmetry is not checked
|
||||||
|
void eivec_sym(Vec3<T> &w, Mat3 &v) const; //only for real symmetric matrix, symmetry is not checked
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user