/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q)
{
float v1[3], v2[3];
float w, x, y, z, c, s, d;
int col;
/* If rank(M) is 2, we should find a non-zero column in MadjT */
col = find_max_col(MadjT);
if (col<0) {
do_rank1(M, Q); /* Rank<2 */
return;
}
v1[0] = MadjT[0][col];
v1[1] = MadjT[1][col];
v1[2] = MadjT[2][col];
make_reflector(v1, v1);
reflect_cols(M, v1);
vcross(M[0], M[1], v2);
make_reflector(v2, v2);
reflect_rows(M, v2);
w = M[0][0];
x = M[0][1];
y = M[1][0];
z = M[1][1];
if (w*z>x*y) {
c = z+w;
s = y-x;
d = static_cast<float>(sqrt(c*c+s*s));
c = c/d;
s = s/d;
Q[0][0] = Q[1][1] = c;
Q[0][1] = -(Q[1][0] = s);
} else {
c = z-w;
s = y+x;
d = static_cast<float>(sqrt(c*c+s*s));
c = c/d;
s = s/d;
Q[0][0] = -(Q[1][1] = c);
Q[0][1] = Q[1][0] = s;
}
Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0;
Q[2][2] = 1.0;
reflect_cols(Q, v1);
reflect_rows(Q, v2);
}
// Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose
void do_rank2(mat3& M, mat3 &MadjT, mat3& Q)
{
vec3 v1, v2;
nv_scalar w, x, y, z, c, s, d;
int col;
// If rank(M) is 2, we should find a non-zero column in MadjT
col = find_max_col(MadjT);
if (col<0)
{
do_rank1(M, Q);
return;
} // Rank<2
v1[0] = MadjT[0][col];
v1[1] = MadjT[1][col];
v1[2] = MadjT[2][col];
make_reflector(v1, v1);
reflect_cols(M, v1);
cross(v2, M[0], M[1]);
make_reflector(v2, v2);
reflect_rows(M, v2);
w = M[0][0];
x = M[0][1];
y = M[1][0];
z = M[1][1];
if (w*z>x*y)
{
c = z+w; s = y-x; d = sqrtf(c*c+s*s); c = c/d; s = s/d;
Q(0,0) = Q(1,1) = c; Q(0,1) = -(Q(1,0) = s);
}
else
{
c = z-w; s = y+x; d = sqrtf(c*c+s*s); c = c/d; s = s/d;
Q(0,0) = -(Q(1,1) = c); Q(0,1) = Q(1,0) = s;
}
Q(0,2) = Q(2,0) = Q(1,2) = Q(2,1) = nv_zero; Q(2,2) = nv_one;
reflect_cols(Q, v1);
reflect_rows(Q, v2);
}