vecmat3: diagonalization replaced by Jacobi to avoid numerical errors when already close to diagonal

This commit is contained in:
Jiri Pittner 2024-01-26 23:03:04 +01:00
parent 1ee984eda2
commit c77c112dbf
3 changed files with 152 additions and 23 deletions

10
t.cc
View File

@ -2461,7 +2461,7 @@ Polynomial<double> pp({1,2,3,4,5});
cout<<pp; cout<<pp;
} }
if(0) if(1)
{ {
//prepare random symmetric mat3 //prepare random symmetric mat3
int seed; int seed;
@ -2470,10 +2470,14 @@ if(sizeof(int)!=read(f,&seed,sizeof(int))) laerror("cannot read /dev/random");
close(f); close(f);
srand(seed); srand(seed);
double scale;
cin >> scale;
NRMat<double> tmp(3,3); NRMat<double> tmp(3,3);
tmp.randomize(2.); tmp.randomize(scale);
Mat3<double> mm(tmp); Mat3<double> mm(tmp);
mm.symmetrize(); mm.symmetrize();
mm+= 1.;
NRMat<double> m(&mm[0][0],3,3); NRMat<double> m(&mm[0][0],3,3);
cout <<m<<"3 3\n"<<mm<<endl; cout <<m<<"3 3\n"<<mm<<endl;
@ -3149,7 +3153,7 @@ for(int i=0; i<a.size(); ++i)
} }
} }
if(1) if(0)
{ {
} }

View File

@ -91,17 +91,24 @@ template<typename T>
void Vec3<T>::inertia(Mat3<T> &m, const T weight) const void Vec3<T>::inertia(Mat3<T> &m, const T weight) const
{ {
T r2 = q[0]*q[0]+q[1]*q[1]+q[2]*q[2]; T r2 = q[0]*q[0]+q[1]*q[1]+q[2]*q[2];
T tmp;
m[0][0] -= weight*(q[0]*q[0]-r2); m[0][0] -= weight*(q[0]*q[0]-r2);
m[0][1] -= weight*q[0]*q[1];
m[0][2] -= weight*q[0]*q[2];
m[1][0] -= weight*q[1]*q[0]; tmp= weight*q[0]*q[1];
m[0][1] -= tmp;
m[1][0] -= tmp;
tmp = weight*q[0]*q[2];
m[0][2] -= tmp;
m[2][0] -= tmp;
m[1][1] -= weight*(q[1]*q[1]-r2); m[1][1] -= weight*(q[1]*q[1]-r2);
m[1][2] -= weight*q[1]*q[2];
m[2][0] -= weight*q[2]*q[0]; tmp = weight*q[1]*q[2];
m[2][1] -= weight*q[2]*q[1]; m[1][2] -= tmp;
m[2][1] -= tmp;
m[2][2] -= weight*(q[2]*q[2]-r2); m[2][2] -= weight*(q[2]*q[2]-r2);
} }
@ -671,6 +678,14 @@ else for(int i=0;i<3;++i) for(int j=0; j<3; ++j) q[i][j]=x*RANDDOUBLESIGNED();
#endif #endif
#if 0
//the Kopp's method is numerically unstable if the matrix is already close to diagonal
//for example repeated diagonalization of inertia tensor and performing a body rotation
//yields nonsense on the second run
//I switched to Jacobi
//
//eigensolver for 3x3 matrix by Joachim Kopp - analytic formula version, //eigensolver for 3x3 matrix by Joachim Kopp - analytic formula version,
//might be unstable for ill-conditioned ones, then use other methods //might be unstable for ill-conditioned ones, then use other methods
//cf. arxiv physics 0610206v3 //cf. arxiv physics 0610206v3
@ -703,7 +718,7 @@ else for(int i=0;i<3;++i) for(int j=0; j<3; ++j) q[i][j]=x*RANDDOUBLESIGNED();
#define SQR(x) ((x)*(x)) // x^2 #define SQR(x) ((x)*(x)) // x^2
// //
template <typename T> template <typename T>
void Mat3<T>::eival_sym(Vec3<T> &w, const bool sortdown) const static void eival_sym(const Mat3<T> &q, Vec3<T> &w, const bool sortdown)
{ {
T m, c1, c0; T m, c1, c0;
@ -721,13 +736,13 @@ T m, c1, c0;
c0 = q[2][2]*dd + q[0][0]*ee + q[1][1]*ff - q[0][0]*q[1][1]*q[2][2] 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) - 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; T p, sqrt_p, Q, c, s, phi;
p = SQR(m) - 3.0*c1; p = SQR(m) - 3.0*c1;
q = m*(p - (3.0/2.0)*c1) - (27.0/2.0)*c0; Q = m*(p - (3.0/2.0)*c1) - (27.0/2.0)*c0;
sqrt_p = sqrt(abs(p)); sqrt_p = sqrt(abs(p));
phi = 27.0 * ( 0.25*SQR(c1)*(p - c1) + c0*(q + 27.0/4.0*c0)); 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); phi = (1.0/3.0) * atan2(sqrt(abs(phi)), Q);
c = sqrt_p*cos(phi); c = sqrt_p*cos(phi);
s = (1.0/M_SQRT3)*sqrt_p*sin(phi); s = (1.0/M_SQRT3)*sqrt_p*sin(phi);
@ -778,7 +793,7 @@ else
// v1.0: First released version // v1.0: First released version
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
template <typename T> template <typename T>
void Mat3<T>::eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown) const bool Mat3<T>::eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown) const
{ {
T norm; // Squared norm or inverse norm of current eigenvector T norm; // Squared norm or inverse norm of current eigenvector
T n0, n1; // Norm of first and second columns of A T n0, n1; // Norm of first and second columns of A
@ -790,13 +805,13 @@ void Mat3<T>::eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown) const
int i, j; // Loop counters int i, j; // Loop counters
// Calculate eigenvalues // Calculate eigenvalues
eival_sym(w,sortdown); eival_sym(*this, w,sortdown);
Mat3<T> A(*this); //scratch copy Mat3<T> A(*this); //scratch copy
wmax = fabs(w[0]); wmax = abs(w[0]);
if ((t=fabs(w[1])) > wmax) if ((t=abs(w[1])) > wmax)
wmax = t; wmax = t;
if ((t=fabs(w[2])) > wmax) if ((t=abs(w[2])) > wmax)
wmax = t; wmax = t;
thresh = SQR(8.0 * DBL_EPSILON * wmax); thresh = SQR(8.0 * DBL_EPSILON * wmax);
@ -857,7 +872,7 @@ Mat3<T> A(*this); //scratch copy
// Prepare calculation of second eigenvector // Prepare calculation of second eigenvector
t = w[0] - w[1]; t = w[0] - w[1];
if (fabs(t) > 8.0 * DBL_EPSILON * wmax) if (abs(t) > 8.0 * DBL_EPSILON * wmax)
{ {
// For non-degenerate eigenvalue, calculate second eigenvector by the formula // For non-degenerate eigenvalue, calculate second eigenvector by the formula
// v[1] = (A - w[1]).e1 x (A - w[1]).e2 // v[1] = (A - w[1]).e1 x (A - w[1]).e2
@ -959,11 +974,122 @@ Mat3<T> A(*this); //scratch copy
v[1][2] = v[2][0]*v[0][1] - v[0][0]*v[2][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]; v[2][2] = v[0][0]*v[1][1] - v[1][0]*v[0][1];
return 0;
} }
#undef SQR #undef SQR
//end eigensolver for 3x3 matrix //end Kopp's eigensolver for 3x3 matrix
///////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////
#else
//begin Jacobi's eigensolver for 3x3 matrix
#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau);
#define SWAP(i,j) {T tmp; tmp=d[i]; d[i]=d[j]; d[j]=tmp; for(int ii=0; ii<3; ++ii) {tmp=v[ii][i]; v[ii][i]=v[ii][j]; v[ii][j]=tmp;}}
template <typename T>
bool Mat3<T>::eivec_sym(Vec3<T> &d, Mat3 &v, const bool sortdown) const
{
Mat3<T> a(*this);//will be overwritten but we are const
const int n=3;
int j,iq,ip,i;
T tresh,theta,tau,t,sm,s,h,g,c;
Vec3<T> b,z;
for (ip=0;ip<n;ip++) {
for (iq=0;iq<n;iq++) v[ip][iq]=0.0;
v[ip][ip]=1.0;
}
for (ip=0;ip<n;ip++) {
b[ip]=d[ip]=a[ip][ip];
z[ip]=0.0;
}
for (i=1;i<=50;i++) {
sm=0.0;
for (ip=0;ip<n-1;ip++) {
for (iq=ip+1;iq<n;iq++)
sm += abs(a[ip][iq]);
}
if (sm == 0.0)
{
if(sortdown)
{
if(d[0]<d[1]) SWAP(0,1)
if(d[0]<d[2]) SWAP(0,2)
if(d[1]<d[2]) SWAP(1,2)
}
else
{
if(d[0]>d[1]) SWAP(0,1)
if(d[0]>d[2]) SWAP(0,2)
if(d[1]>d[2]) SWAP(1,2)
}
return 0;
}
if (i < 4)
tresh=0.2*sm/(n*n);
else
tresh=0.0;
for (ip=0;ip<n-1;ip++) {
for (iq=ip+1;iq<n;iq++) {
g=100.0*abs(a[ip][iq]);
if (i > 4 && abs(d[ip])+g == abs(d[ip])
&& abs(d[iq])+g == abs(d[iq]))
a[ip][iq]=0.0;
else if (abs(a[ip][iq]) > tresh) {
h=d[iq]-d[ip];
if (abs(h)+g == abs(h))
t=(a[ip][iq])/h;
else {
theta=0.5*h/(a[ip][iq]);
t=1.0/(abs(theta)+sqrt(1.0+theta*theta));
if (theta < 0.0) t = -t;
}
c=1.0/sqrt(1+t*t);
s=t*c;
tau=s/(1.0+c);
h=t*a[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
a[ip][iq]=0.0;
for (j=0;j<=ip-1;j++) {
ROTATE(a,j,ip,j,iq)
}
for (j=ip+1;j<=iq-1;j++) {
ROTATE(a,ip,j,j,iq)
}
for (j=iq+1;j<n;j++) {
ROTATE(a,ip,j,iq,j)
}
for (j=0;j<n;j++) {
ROTATE(v,j,ip,j,iq)
}
}
}
}
for (ip=0;ip<n;ip++) {
b[ip] += z[ip];
d[ip]=b[ip];
z[ip]=0.0;
}
}
return 1;
}
#undef ROTATE
#undef SWAP
#endif
//end Jacobi's eigensolver for 3x3 matrix
/////////////////////////////////////////////////////////////////////////////////////////////
template<typename T> template<typename T>
T Mat3<T>::norm(const T scalar) const T Mat3<T>::norm(const T scalar) const

View File

@ -183,8 +183,7 @@ public:
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 symmetrize(); //average offdiagonal elements
void eival_sym(Vec3<T> &w, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked bool eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked, returns false on success
void eivec_sym(Vec3<T> &w, Mat3 &v, const bool sortdown=false) const; //only for real symmetric matrix, symmetry is not checked
T norm(const T scalar = 0) const; T norm(const T scalar = 0) const;
void qrd(Mat3 &q, Mat3 &r); //not const, destroys the matrix void qrd(Mat3 &q, Mat3 &r); //not const, destroys the matrix
void svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations=false) const; //if proper_rotations = true, singular value can be negative but u and v are proper rotations void svd(Mat3 &u, Vec3<T> &w, Mat3 &v, bool proper_rotations=false) const; //if proper_rotations = true, singular value can be negative but u and v are proper rotations