vector sqrtvec(const vector& v)
{
int Sz=v.Sz;
vector sqrtv(Sz);
for (int i=0;i<Sz;i++) sqrtv.TheVector[i]=sqrt(v.TheVector[i]);
if (!sqrtv.TheVector) exit(1);
return sqrtv;
}
void jacobi2Sided(real (*A)[3], real (*U)[3], real (*V)[3]) {
int p, q;
real w, x, y, z, m1, m2, c, s, r, c1, c2, s1, s2, r2, t2, d1, d2, k, tmp;
int flg;
int cnt = 3;
while (cnt--) {
for (p = 1; p < 3; p++) {
for (q = 0; q < p; q++) {
w = A[p][p];
x = A[p][q];
y = A[q][p];
z = A[q][q];
flg = 0;
if (y == 0 && z == 0) {
y = x;
x = 0;
flg = 1;
}
m1 = w + z;
m2 = x - y;
if (fabsv(m2) <= TOL * fabsv(m1)) {
c = 1;
s = 0;
} else {
r = m1 / m2;
s = SIGN(r) / sqrtv(1 + r * r);
c = s * r;
}
m1 = s * (x + y) + c * (z - w);
m2 = 2 * (c * x - s * z);
if (fabsv(m2) <= TOL * fabsv(m1)) {
c2 = 1;
s2 = 0;
} else {
r2 = m1 / m2;
t2 = SIGN(r2) / (fabsv(r2) + sqrtv(1 + r2 * r2)); //%use hypot..?
c2 = 1 / sqrtv(1 + t2 * t2);
s2 = c2 * t2;
}
c1 = c2 * c - s2 * s;
s1 = s2 * c + c2 * s;
if (flg == 1) {
c2 = c1;
s2 = s1;
c1 = 1;
s1 = 0;
}
d1 = c1 * (w * c2 - x * s2) - s1 * (y * c2 - z * s2);
d2 = s1 * (w * s2 + x * c2) + c1 * (y * s2 + z * c2);
if (fabsv(d1) > fabsv(d2)) {
tmp = c1;
c1 = -s1;
s1 = tmp;
tmp = c2;
c2 = -s2;
s2 = tmp;
tmp = d2;
d2 = d1;
d1 = tmp;
}
k = 1;
if (d1 < 0) {
c1 = -c1;
s1 = -s1;
k = -k;
}
if (d2 < 0) {
k = -k;
}
apply_jacobi_L(A, p, q, c1, s1, k);
apply_jacobi_R(A, p, q, c2, s2, 1);
apply_jacobi_R(U, p, q, c1, s1, k);
apply_jacobi_R(V, p, q, c2, s2, 1);
}
}
}
}
