/* Polar Decomposition of 3x3 matrix in 4x4,
* M = QS. See Nicholas Higham and Robert S. Schreiber,
* Fast Polar Decomposition of An Arbitrary Matrix,
* Technical Report 88-942, October 1988,
* Department of Computer Science, Cornell University.
*/
double polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
{
#define TOL 1.0e-6
HMatrix Mk, MadjTk, Ek;
double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
int i, j;
mat_tpose(Mk,=,M,3);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
do {
adjoint_transpose(Mk, MadjTk);
det = vdot(Mk[0], MadjTk[0]);
if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
g1 = gamma*0.5;
g2 = 0.5/(gamma*det);
mat_copy(Ek,=,Mk,3);
mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3);
mat_copy(Ek,-=,Mk,3);
E_one = norm_one(Ek);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
} while (E_one>(M_one*TOL));
mat_tpose(Q,=,Mk,3); mat_pad(Q);
mat_mult(Mk, M, S); mat_pad(S);
for (i=0; i<3; i++) for (j=i; j<3; j++)
S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
return (det);
}
int normalize_one(deque<double> & a) {
double norm=norm_one(a);
for(int i=0; i<a.size(); i++) {
a[i]/=norm;
}
return 0;
}
void PolarDecomposer::PolarDecompose(const mat3& A, mat3& Q, mat3& S, double& det, double tolerance)
{
mat3 At = A.transpose();
mat3 Aadj;
mat3 Ek;
double A_one = norm_one(At);
double A_inf = norm_inf(At);
double Aadj_one, Aadj_inf, E_one, gamma, g1, g2;
do
{
Aadj = mat3(At[1].Cross(At[2]), At[2].Cross(At[0]), At[0].Cross(At[1]));
det = At[0][0] * Aadj[0][0] + At[0][1] * Aadj[0][1] + At[0][2] * Aadj[0][2];
if(det == 0.0)
{
//TODO: handle this case
printf("Warning: zero determinant encountered.\n");
break;
}
Aadj_one = norm_one(Aadj);
Aadj_inf = norm_inf(Aadj);
gamma = sqrt(sqrt((Aadj_one*Aadj_inf)/(A_one*A_inf))/fabs(det));
g1 = gamma * 0.5;
g2 = 0.5/(gamma*det);
for(unsigned int i = 0; i < 3; i++)
for(unsigned int j = 0; j < 3; j++)
{
Ek[i][j] = At[i][j];
At[i][j] = g1 * At[i][j] + g2 * Aadj[i][j];
Ek[i][j] -= At[i][j];
}
E_one = norm_one(Ek);
A_one = norm_one(At);
A_inf = norm_inf(At);
}
while( E_one > A_one * tolerance);
if(fabs(det) < EPSILON)//edit by Xing
Q = mat3::Identity();
else
Q = At.transpose();
//TODO: if S is to be used. uncomment this part
for(unsigned int i = 0; i < 3; i++)
for(unsigned int j = 0; j < 3; j++)
{
S[i][j] = 0;
for(unsigned int k = 0; k < 3; k++)
S[i][j] += At[i][k] * A[k][j];
}
for(unsigned int i = 0; i < 3; i++)
for(unsigned int j = i; j < 3; j++)
{
S[i][j] = S[j][i] = 0.5*(S[i][j] + S[j][i]);
}
}
