/** Find orthogonal factor Q of rank 1 (or less) M **/ void do_rank1(HMatrix M, HMatrix Q) { float v1[3], v2[3], s; int col; /* If rank(M) is 1, we should find a non-zero column in M */ col = find_max_col(M); if (col<0) {mat_copy(Q,=,mat_id,4); return;} /* Rank is 0 */
/** 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); }