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); }