/** Find orthogonal factor Q of rank 1 (or less) M **/
void do_rank1(_HMatrix M, _HMatrix Q)
{
double v1[3], v2[3], s;
int col;
mat_copy(Q,=,mat_id,4);
/* If rank(M) is 1, we should find a non-zero column in M */
col = find_max_col(M);
if (col<0) return; /* Rank is 0 */
v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
make_reflector(v1, v1); reflect_cols(M, v1);
v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
make_reflector(v2, v2); reflect_rows(M, v2);
s = M[2][2];
if (s<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(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);
}
// Find orthogonal factor Q of rank 1 (or less) M
void do_rank1(mat3 & M, mat3& Q)
{
vec3 v1, v2;
nv_scalar s;
int col;
Q = mat3_id;
/* If rank(M) is 1, we should find a non-zero column in M */
col = find_max_col(M);
if ( col < 0 )
return; /* Rank is 0 */
v1 = M.col(col);
make_reflector(v1, v1);
reflect_cols(M, v1);
v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
make_reflector(v2, v2); reflect_rows(M, v2);
s = M[2][2];
if (s < nv_zero)
Q(2,2) = -nv_one;
reflect_cols(Q, v1); reflect_rows(Q, v2);
}