You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
286 lines
8.2 KiB
286 lines
8.2 KiB
2 years ago
|
/*
|
||
|
|
* Copyright(C) 1999-2020, 2023 National Technology & Engineering Solutions
|
||
|
|
* of Sandia, LLC (NTESS). Under the terms of Contract DE-NA0003525 with
|
||
|
|
* NTESS, the U.S. Government retains certain rights in this software.
|
||
|
|
*
|
||
|
|
* See packages/seacas/LICENSE for details
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "defs.h"
|
||
|
|
#include <math.h>
|
||
|
|
|
||
|
|
/* Find eigenvalues of 3x3 symmetric system by solving cubic. */
|
||
|
|
void ch_evals3(double H[3][3], /* 3x3 sym matrix for lowest eigenvalue */
|
||
|
|
double *eval1, /* smallest eigenvalue */
|
||
|
|
double *eval2, /* middle eigenvalue */
|
||
|
|
double *eval3 /* largest eigenvalue */
|
||
|
|
)
|
||
|
|
{
|
||
|
|
double mat[3][3] = {{0.0}}; /* scaled version of H */
|
||
|
|
double a1, a2, a3; /* coefficients of cubic equation */
|
||
|
|
double q, r; /* intermediate terms */
|
||
|
|
double q3, r2; /* powers of q and r */
|
||
|
|
double theta; /* angular parameter */
|
||
|
|
double root1, root2, root3; /* three roots of cubic */
|
||
|
|
double tol = 1.0e-6; /* allowed deviation */
|
||
|
|
double xmax; /* largest matrix element for scaling */
|
||
|
|
int i, j; /* loop indices */
|
||
|
|
|
||
|
|
/* This first requires solving a cubic equation. */
|
||
|
|
/* Normalize to avoid any numerical problems. */
|
||
|
|
xmax = 0.0;
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
for (j = i; j < 3; j++) {
|
||
|
|
if (fabs(H[i][j]) > xmax) {
|
||
|
|
xmax = fabs(H[i][j]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (xmax != 0) {
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
for (j = 0; j < 3; j++) {
|
||
|
|
mat[i][j] = H[i][j] / xmax;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
a1 = -(mat[0][0] + mat[1][1] + mat[2][2]);
|
||
|
|
a2 = (mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]) +
|
||
|
|
(mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) +
|
||
|
|
(mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]);
|
||
|
|
a3 = -determinant(mat, 3);
|
||
|
|
|
||
|
|
if (a3 == 0) {
|
||
|
|
root1 = 0;
|
||
|
|
/* Solve quadratic. */
|
||
|
|
q = -.5 * (a1 + sign(a1) * sqrt(a1 * a1 - 4 * a2));
|
||
|
|
root2 = q;
|
||
|
|
root3 = a2 / q;
|
||
|
|
}
|
||
|
|
|
||
|
|
else { /* solve cubic */
|
||
|
|
q = (a1 * a1 - 3 * a2) / 9;
|
||
|
|
r = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) / 54;
|
||
|
|
q3 = q * q * q;
|
||
|
|
r2 = r * r;
|
||
|
|
|
||
|
|
/* To avoid missing a root, check for roundoff. */
|
||
|
|
if ((q3 < r2) && fabs(q3 - r2) < tol * (fabs(q3) + fabs(r2))) {
|
||
|
|
q3 = r2;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (q3 >= r2) { /* Three real roots. */
|
||
|
|
if (r == 0) {
|
||
|
|
theta = HALFPI;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
q3 = sqrt(q3);
|
||
|
|
if (q3 < fabs(r)) {
|
||
|
|
q3 = fabs(r);
|
||
|
|
}
|
||
|
|
theta = acos(r / q3);
|
||
|
|
}
|
||
|
|
q = -2 * sqrt(q);
|
||
|
|
|
||
|
|
root1 = q * cos(theta / 3) - a1 / 3;
|
||
|
|
root2 = q * cos((theta + TWOPI) / 3) - a1 / 3;
|
||
|
|
root3 = q * cos((theta + 2 * TWOPI) / 3) - a1 / 3;
|
||
|
|
}
|
||
|
|
else { /* Only one real root. */
|
||
|
|
theta = sqrt(r2 - q3) + fabs(r);
|
||
|
|
theta = pow(theta, 1.0 / 3.0);
|
||
|
|
|
||
|
|
root1 = root2 = root3 = -sign(r) * (theta + q / theta) - a1 / 3;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
root1 *= xmax;
|
||
|
|
root2 *= xmax;
|
||
|
|
root3 *= xmax;
|
||
|
|
*eval1 = min(root1, root2);
|
||
|
|
*eval1 = min(*eval1, root3);
|
||
|
|
*eval3 = max(root1, root2);
|
||
|
|
*eval3 = max(*eval3, root3);
|
||
|
|
if (root1 != *eval1 && root1 != *eval3) {
|
||
|
|
*eval2 = root1;
|
||
|
|
}
|
||
|
|
else if (root2 != *eval1 && root2 != *eval3) {
|
||
|
|
*eval2 = root2;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
*eval2 = root3;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void kramer3( /* Use Kramer's rule to solve 3x3 */
|
||
|
|
double A[3][3], double b[3], double x[3] /* Solve Ax=b */
|
||
|
|
)
|
||
|
|
{
|
||
|
|
double det = 1.0 / determinant(A, 3);
|
||
|
|
|
||
|
|
x[0] = (b[0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) -
|
||
|
|
b[1] * (A[0][1] * A[2][2] - A[0][2] * A[2][1]) +
|
||
|
|
b[2] * (A[0][1] * A[1][2] - A[0][2] * A[1][1])) *
|
||
|
|
det;
|
||
|
|
|
||
|
|
x[1] = -(b[0] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) -
|
||
|
|
b[1] * (A[0][0] * A[2][2] - A[0][2] * A[2][0]) +
|
||
|
|
b[2] * (A[0][0] * A[1][2] - A[0][2] * A[1][0])) *
|
||
|
|
det;
|
||
|
|
|
||
|
|
x[2] = (b[0] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]) -
|
||
|
|
b[1] * (A[0][0] * A[2][1] - A[0][1] * A[2][0]) +
|
||
|
|
b[2] * (A[0][0] * A[1][1] - A[0][1] * A[1][0])) *
|
||
|
|
det;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Find the eigenvector of symmetric 3x3 matrix w/ given eigenvalue. */
|
||
|
|
void ch_eigenvec3(double A[3][3], /* matrix to find eigenvector of */
|
||
|
|
double eval, /* eigenvalue */
|
||
|
|
double evec[3], /* eigenvector returned */
|
||
|
|
double *res /* returned error estimate */
|
||
|
|
)
|
||
|
|
{
|
||
|
|
double mat[3][3]; /* copy of A to write over */
|
||
|
|
int ind[3]; /* permutation indices */
|
||
|
|
double ex, ey, ez; /* elements of eigenvector returned */
|
||
|
|
double xmax; /* maximum value in matrix */
|
||
|
|
double tmp; /* intermediate values */
|
||
|
|
double norm; /* norm of eigenvector */
|
||
|
|
double res1, res2, res3; /* elements of residual vector */
|
||
|
|
double tol = 1.0e-6; /* smaller value assumed to be zero */
|
||
|
|
int imax = -1, jmax = -1; /* indices of max value in matrix */
|
||
|
|
int i, j; /* loop counters */
|
||
|
|
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
for (j = 0; j < 3; j++) {
|
||
|
|
mat[i][j] = A[i][j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
mat[i][i] -= eval;
|
||
|
|
}
|
||
|
|
|
||
|
|
ind[0] = 0;
|
||
|
|
ind[1] = 1;
|
||
|
|
ind[2] = 2;
|
||
|
|
|
||
|
|
/* Find the largest element in the matrix. */
|
||
|
|
xmax = 0.0;
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
for (j = i; j < 3; j++) {
|
||
|
|
if (fabs(mat[i][j]) > xmax) {
|
||
|
|
imax = i;
|
||
|
|
jmax = j;
|
||
|
|
xmax = fabs(mat[i][j]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (xmax == 0.0) { /* Handle completely degenerate case first. */
|
||
|
|
evec[0] = 1.0;
|
||
|
|
evec[1] = evec[2] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
else {
|
||
|
|
/* Scale the matrix so largest value is 1.0 */
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
for (j = 0; j < 3; j++) {
|
||
|
|
mat[i][j] /= xmax;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Swap rows if necessary to move max element to first row. */
|
||
|
|
if (imax != 0) {
|
||
|
|
for (j = 0; j < 3; j++) {
|
||
|
|
tmp = mat[0][j];
|
||
|
|
mat[0][j] = mat[imax][j];
|
||
|
|
mat[imax][j] = tmp;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Swap columns if necessary to move max element to first position. */
|
||
|
|
if (jmax != 0) {
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
tmp = mat[i][0];
|
||
|
|
mat[i][0] = mat[i][jmax];
|
||
|
|
mat[i][jmax] = tmp;
|
||
|
|
}
|
||
|
|
ind[0] = jmax;
|
||
|
|
ind[jmax] = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Reduce matrix to 2x2 by subtracting off first row. */
|
||
|
|
for (i = 1; i < 3; i++) {
|
||
|
|
for (j = 1; j < 3; j++) {
|
||
|
|
mat[i][j] = mat[0][0] * mat[i][j] - mat[i][0] * mat[0][j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Find maximum element in reduced 2x2 matrix. */
|
||
|
|
xmax = 0.0;
|
||
|
|
for (i = 1; i < 3; i++) {
|
||
|
|
for (j = i; j < 3; j++) {
|
||
|
|
if (fabs(mat[i][j]) > xmax) {
|
||
|
|
imax = i;
|
||
|
|
jmax = j;
|
||
|
|
xmax = fabs(mat[i][j]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (xmax < tol) { /* Handle 2-fold degenerate case - skip to end. */
|
||
|
|
ey = 1.0;
|
||
|
|
ex = ez = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
else {
|
||
|
|
/* Swap rows 2 and 3 to move max element to 2nd row. */
|
||
|
|
if (imax != 1) {
|
||
|
|
for (j = 0; j < 3; j++) {
|
||
|
|
tmp = mat[1][j];
|
||
|
|
mat[1][j] = mat[imax][j];
|
||
|
|
mat[imax][j] = tmp;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Swap columns to move max element to (1,1) position. */
|
||
|
|
if (jmax != 1) {
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
tmp = mat[i][1];
|
||
|
|
mat[i][1] = mat[i][2];
|
||
|
|
mat[i][2] = tmp;
|
||
|
|
}
|
||
|
|
i = ind[1];
|
||
|
|
ind[1] = ind[2];
|
||
|
|
ind[2] = i;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Compute eigenvector from numerically stabilized matrix. */
|
||
|
|
ez = mat[0][0] * mat[1][1];
|
||
|
|
ey = -mat[1][2] * mat[0][0];
|
||
|
|
ex = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
||
|
|
}
|
||
|
|
/* Reorder the e-vector to undo pivoting - end of 3-D case. */
|
||
|
|
evec[ind[0]] = ex;
|
||
|
|
evec[ind[1]] = ey;
|
||
|
|
evec[ind[2]] = ez;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Normalize eigenvector and calculate a normalized eigen-residual. */
|
||
|
|
norm = sqrt(evec[0] * evec[0] + evec[1] * evec[1] + evec[2] * evec[2]);
|
||
|
|
for (i = 0; i < 3; i++) {
|
||
|
|
evec[i] /= norm;
|
||
|
|
}
|
||
|
|
res1 = (A[0][0] - eval) * evec[0] + A[0][1] * evec[1] + A[0][2] * evec[2];
|
||
|
|
res2 = A[1][0] * evec[0] + (A[1][1] - eval) * evec[1] + A[1][2] * evec[2];
|
||
|
|
res3 = A[2][0] * evec[0] + A[2][1] * evec[1] + (A[2][2] - eval) * evec[2];
|
||
|
|
*res = sqrt(res1 * res1 + res2 * res2 + res3 * res3);
|
||
|
|
|
||
|
|
/* Now normalize the residual */
|
||
|
|
res1 = fabs(A[0][0]) + fabs(A[0][1]) + fabs(A[0][2]);
|
||
|
|
res2 = fabs(A[1][0]) + fabs(A[1][1]) + fabs(A[1][2]);
|
||
|
|
res3 = fabs(A[2][0]) + fabs(A[2][1]) + fabs(A[2][2]);
|
||
|
|
res2 = max(res2, res3);
|
||
|
|
*res /= max(res1, res2);
|
||
|
|
}
|