void QRDecomposition(glm::mat3 B, glm::mat3 &Q, glm::mat3 &R)
{
// QR decomposition of 3x3 matrices using Givens rotations to
// eliminate elements B21, B31, B32
glm::quat qQ; // cumulative rotation
glm::quat qU; // each Givens rotation in quaternion form
glm::mat3 U;
float ch, sh;
QRGivensQuaternion(B[0][0],B[0][1],ch,sh);
qU = glm::quat(ch,0,0,sh);
U = glm::toMat3(qU);
B = glm::transpose(U) * B;
// update cumulative rotation
qQ *= qU;
// second givens rotation
QRGivensQuaternion(B[0][0],B[0][2],ch,sh);
qU = glm::quat(ch,0,-sh,0);
U = glm::toMat3(qU);
B = glm::transpose(U) * B;
qQ *= qU;
// third Givens rotation
QRGivensQuaternion(B[1][1],B[1][2],ch,sh);
qU = glm::quat(ch,sh,0,0);
U = glm::toMat3(qU);
B = glm::transpose(U) * B;
qQ *= qU;
// B has been transformed into R
R = B;
// qQ now contains final rotation for Q
Q = glm::toMat3(qQ);
}
// B matrix that we want to decompose
// output Q
// output R
void
mtx_dgeqrf3(double *B, double *Q, double *R)
{
double ch1, sh1, ch2, sh2, ch3, sh3;
double a, b;
// first givens rotation (ch,0,0,sh)
QRGivensQuaternion(B[0],B[3],&ch1,&sh1);
a = 1.0 - 2.0*sh1*sh1;
b = 2.0*ch1*sh1;
// apply B = Q' * B
R[0] = a * B[0] + b * B[3];
R[1] = a * B[1] + b * B[4];
R[2] = a * B[2] + b * B[5];
R[3] = -b * B[0] + a * B[3];
R[4] = -b * B[1] + a * B[4];
R[5] = -b * B[2] + a * B[5];
R[6] = B[6];
R[7] = B[7];
R[8] = B[8];
// second givens rotation (ch,0,-sh,0)
QRGivensQuaternion(R[0],R[6],&ch2,&sh2);
a = 1.0 - 2.0*sh2*sh2;
b = 2.0*ch2*sh2;
// apply B = Q' * B;
B[0] = a * R[0] + b * R[6];
B[1] = a * R[1] + b * R[7];
B[2] = a * R[2] + b * R[8];
B[3] = R[3];
B[4] = R[4];
B[5] = R[5];
B[6] = -b * R[0] + a * R[6];
B[7] = -b * R[1] + a * R[7];
B[8] = -b * R[2] + a * R[8];
// third givens rotation (ch,sh,0,0)
QRGivensQuaternion(B[4],B[7],&ch3,&sh3);
a = 1.0 - 2.0*sh3*sh3;
b = 2.0*ch3*sh3;
// R is now set to desired value
R[0] = B[0];
R[1] = B[1];
R[2] = B[2];
R[3] = a * B[3] + b * B[6];
R[4] = a * B[4] + b * B[7];
R[5] = a * B[5] + b * B[8];
R[6] = -b * B[3] + a * B[6];
R[7] = -b * B[4] + a * B[7];
R[8] = -b * B[5] + a * B[8];
// construct the cumulative rotation Q=Q1 * Q2 * Q3
// the number of floating point operations for three quaternion multiplications
// is more or less comparable to the explicit form of the joined matrix.
// certainly more memory-efficient!
double sh12=sh1*sh1;
double sh22=sh2*sh2;
double sh32=sh3*sh3;
Q[0] = (-1+2*sh12)*(-1+2*sh22);
Q[1] = 4*ch2*ch3*(-1+2*sh12)*sh2*sh3+2*ch1*sh1*(-1+2*sh32);
Q[2] = 4*ch1*ch3*sh1*sh3-2*ch2*(-1+2*sh12)*sh2*(-1+2*sh32);
Q[3] = 2*ch1*sh1*(1-2*sh22);
Q[4] = -8*ch1*ch2*ch3*sh1*sh2*sh3+(-1+2*sh12)*(-1+2*sh32);
Q[5] = -2*ch3*sh3+4*sh1*(ch3*sh1*sh3+ch1*ch2*sh2*(-1+2*sh32));
Q[6] = 2*ch2*sh2;
Q[7] = 2*ch3*(1-2*sh22)*sh3;
Q[8] = (-1+2*sh22)*(-1+2*sh32);
}
