// Slightly modified version of Stan Melax's code for 3x3 matrix diagonalization (Thanks Stan!)
// source: http://www.melax.com/diag.html?attredirects=0
void Diagonalize(const Real (&A)[3][3], Real (&Q)[3][3], Real (&D)[3][3])
{
// A must be a symmetric matrix.
// returns Q and D such that
// Diagonal matrix D = QT * A * Q; and A = Q*D*QT
const int maxsteps=24; // certainly wont need that many.
int k0, k1, k2;
Real o[3], m[3];
Real q [4] = {0.0,0.0,0.0,1.0};
Real jr[4];
Real sqw, sqx, sqy, sqz;
Real tmp1, tmp2, mq;
Real AQ[3][3];
Real thet, sgn, t, c;
for(int i=0;i < maxsteps;++i)
{
// quat to matrix
sqx = q[0]*q[0];
sqy = q[1]*q[1];
sqz = q[2]*q[2];
sqw = q[3]*q[3];
Q[0][0] = (sqx - sqy - sqz + sqw);
Q[1][1] = (-sqx + sqy - sqz + sqw);
Q[2][2] = (-sqx - sqy + sqz + sqw);
tmp1 = q[0]*q[1];
tmp2 = q[2]*q[3];
Q[1][0] = 2.0 * (tmp1 + tmp2);
Q[0][1] = 2.0 * (tmp1 - tmp2);
tmp1 = q[0]*q[2];
tmp2 = q[1]*q[3];
Q[2][0] = 2.0 * (tmp1 - tmp2);
Q[0][2] = 2.0 * (tmp1 + tmp2);
tmp1 = q[1]*q[2];
tmp2 = q[0]*q[3];
Q[2][1] = 2.0 * (tmp1 + tmp2);
Q[1][2] = 2.0 * (tmp1 - tmp2);
// AQ = A * Q
AQ[0][0] = Q[0][0]*A[0][0]+Q[1][0]*A[0][1]+Q[2][0]*A[0][2];
AQ[0][1] = Q[0][1]*A[0][0]+Q[1][1]*A[0][1]+Q[2][1]*A[0][2];
AQ[0][2] = Q[0][2]*A[0][0]+Q[1][2]*A[0][1]+Q[2][2]*A[0][2];
AQ[1][0] = Q[0][0]*A[0][1]+Q[1][0]*A[1][1]+Q[2][0]*A[1][2];
AQ[1][1] = Q[0][1]*A[0][1]+Q[1][1]*A[1][1]+Q[2][1]*A[1][2];
AQ[1][2] = Q[0][2]*A[0][1]+Q[1][2]*A[1][1]+Q[2][2]*A[1][2];
AQ[2][0] = Q[0][0]*A[0][2]+Q[1][0]*A[1][2]+Q[2][0]*A[2][2];
AQ[2][1] = Q[0][1]*A[0][2]+Q[1][1]*A[1][2]+Q[2][1]*A[2][2];
AQ[2][2] = Q[0][2]*A[0][2]+Q[1][2]*A[1][2]+Q[2][2]*A[2][2];
// D = Qt * AQ
D[0][0] = AQ[0][0]*Q[0][0]+AQ[1][0]*Q[1][0]+AQ[2][0]*Q[2][0];
D[0][1] = AQ[0][0]*Q[0][1]+AQ[1][0]*Q[1][1]+AQ[2][0]*Q[2][1];
D[0][2] = AQ[0][0]*Q[0][2]+AQ[1][0]*Q[1][2]+AQ[2][0]*Q[2][2];
D[1][0] = AQ[0][1]*Q[0][0]+AQ[1][1]*Q[1][0]+AQ[2][1]*Q[2][0];
D[1][1] = AQ[0][1]*Q[0][1]+AQ[1][1]*Q[1][1]+AQ[2][1]*Q[2][1];
D[1][2] = AQ[0][1]*Q[0][2]+AQ[1][1]*Q[1][2]+AQ[2][1]*Q[2][2];
D[2][0] = AQ[0][2]*Q[0][0]+AQ[1][2]*Q[1][0]+AQ[2][2]*Q[2][0];
D[2][1] = AQ[0][2]*Q[0][1]+AQ[1][2]*Q[1][1]+AQ[2][2]*Q[2][1];
D[2][2] = AQ[0][2]*Q[0][2]+AQ[1][2]*Q[1][2]+AQ[2][2]*Q[2][2];
o[0] = D[1][2];
o[1] = D[0][2];
o[2] = D[0][1];
m[0] = fabs(o[0]);
m[1] = fabs(o[1]);
m[2] = fabs(o[2]);
k0 = (m[0] > m[1] && m[0] > m[2])?0: (m[1] > m[2])? 1 : 2; // index of largest element of offdiag
k1 = (k0+1)%3;
k2 = (k0+2)%3;
if (o[k0]==0.0)
{
break; // diagonal already
}
thet = (D[k2][k2]-D[k1][k1])/(2.0*o[k0]);
sgn = (thet > 0.0)?1.0:-1.0;
thet *= sgn; // make it positive
t = sgn /(thet +((thet < 1.E6)?sqrt(thet*thet+1.0):thet)) ; // sign(T)/(|T|+sqrt(T^2+1))
c = 1.0/sqrt(t*t+1.0); // c= 1/(t^2+1) , t=s/c
if(c==1.0)
{
break; // no room for improvement - reached machine precision.
}
jr[0 ] = jr[1] = jr[2] = jr[3] = 0.0;
jr[k0] = sgn*sqrt((1.0-c)/2.0); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jr[k0] *= -1.0; // since our quat-to-matrix convention was for v*M instead of M*v
jr[3 ] = sqrt(1.0f - jr[k0] * jr[k0]);
if(jr[3]==1.0)
{
break; // reached limits of floating point precision
}
q[0] = (q[3]*jr[0] + q[0]*jr[3] + q[1]*jr[2] - q[2]*jr[1]);
q[1] = (q[3]*jr[1] - q[0]*jr[2] + q[1]*jr[3] + q[2]*jr[0]);
q[2] = (q[3]*jr[2] + q[0]*jr[1] - q[1]*jr[0] + q[2]*jr[3]);
q[3] = (q[3]*jr[3] - q[0]*jr[0] - q[1]*jr[1] - q[2]*jr[2]);
mq = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] /= mq;
q[1] /= mq;
q[2] /= mq;
q[3] /= mq;
}
}
你需要什麼特定的值?你需要自己的特徵值嗎?分解?解決線性系統?更多細節可能會有所幫助。 – Escualo 2010-12-07 03:16:21
我需要3個特徵值本身,以及最後一個特徵向量。謝謝 – Xzhsh 2010-12-07 04:11:02
您可以使用分析方法,再加上多個精度算術。它應該比基於QR的迭代方法更快,並且應該只包含幾個分支。 – 2014-09-24 12:12:48