[numerics] add a function for computing eigenvalues of a 3x3 symmetrix matrix
@@ -874,4 +874,64 @@ static inline int solve_3x3_gepp(const double* restrict a, double* restrict b)
return info;
}
+/** Computation of the eigenvalues of a symmetric 3x3 real matrix
+ * \param a symmetric column-major matrix (not modified)
+ * \param b a 3x3 work matrix
+ * \param[in,out] eig eigenvalie in decreasing order
+ */
+WARN_RESULT_IGNORED
+static inline int eig_3x3(double* restrict a, double* restrict b, double* restrict eig)
+{
+ SET3X3(a);
+ SET3X3(b);
+ double pi = M_PI;
+ double p1 = *a01* *a01 + *a02* *a02 + *a12* *a12;
+ if (p1 == 0)
+ {
+ /* A is diagonal. */
+ eig[0] = *a00;
+ eig[1] = *a11;
+ eig[2] = *a22;
+ }
+ else
+ {
+ double q = ( *a00+ *a11+ *a22)/3.0;
+ double p2 = ( *a00 - q)*( *a00 - q) + ( *a11 - q)*( *a11 - q) + ( *a22 - q)*( *a22 - q) + 2 * p1;
+ double p = sqrt(p2 / 6.0);
+ *b00 = (1 / p) * ( *a00 - q);
+ *b11 = (1 / p) * ( *a11 - q);
+ *b22 = (1 / p) * ( *a22 - q);
+ *b01 = (1 / p) * ( *a01);
+ *b02 = (1 / p) * ( *a02);
+ *b10 = (1 / p) * ( *a10);
+ *b12 = (1 / p) * ( *a12);
+ *b20 = (1 / p) * ( *a20);
+ *b21 = (1 / p) * ( *a21);
+ //B = (1 / p) * ( *A - q * I) % I is the identity matrix
+ double det =
+ *b00 * *b11 * *b22 + *b01 * *b12 * *b20 + *b02 * *b10 * *b21 -
+ *b00 * *b12 * *b21 - *b01 * *b10 * *b22 - *b02 * *b11 * *b20;
+ double r = det/2.0;
+
+ /* % In exact arithmetic for a symmetric matrix -1 <= r <= 1 */
+ /* % but computation error can leave it slightly outside this range. */
+ double phi;
+ if (r <= -1)
+ phi = pi / 3.0;
+ else if (r >= 1)
+ phi = 0.0;
+ else
+ phi = acos(r) / 3;
+
+ /* % the eigenvalues satisfy eig3 <= eig2 <= eig1 */
+ eig[0] = q + 2 * p * cos(phi);
+ eig[2] = q + 2 * p * cos(phi + (2*pi/3));
+ eig[1] = 3 * q - eig[0] - eig[2]; /* % since trace(A) = eig1 + eig2 + eig3; */
+ }
+
+ return 0;
+}
+
+
+
#endif
