📄 matrix3.cpp
字号:
fT1 = kA[0][1] + fSign * fLength;
afV[2] = kA[0][2] / fT1;
fT2 = -2.0 / (1.0 + afV[2] * afV[2]);
afW[0] = fT2 * (kA[0][1] + kA[0][2] * afV[2]);
afW[1] = fT2 * (kA[1][1] + kA[1][2] * afV[2]);
afW[2] = fT2 * (kA[2][1] + kA[2][2] * afV[2]);
kA[0][1] += afW[0];
kA[1][1] += afW[1];
kA[1][2] += afW[1] * afV[2];
kA[2][1] += afW[2];
kA[2][2] += afW[2] * afV[2];
kR[0][0] = 1.0;
kR[0][1] = kR[1][0] = 0.0;
kR[0][2] = kR[2][0] = 0.0;
kR[1][1] = 1.0 + fT2;
kR[1][2] = kR[2][1] = fT2 * afV[2];
kR[2][2] = 1.0 + fT2 * afV[2] * afV[2];
} else {
kR = Matrix3::identity();
}
// map second column to (*,*,0)
fLength = sqrt(kA[1][1] * kA[1][1] + kA[2][1] * kA[2][1]);
if ( fLength > 0.0 ) {
fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0);
fT1 = kA[1][1] + fSign * fLength;
afV[2] = kA[2][1] / fT1;
fT2 = -2.0 / (1.0 + afV[2] * afV[2]);
afW[1] = fT2 * (kA[1][1] + kA[2][1] * afV[2]);
afW[2] = fT2 * (kA[1][2] + kA[2][2] * afV[2]);
kA[1][1] += afW[1];
kA[1][2] += afW[2];
kA[2][2] += afV[2] * afW[2];
float fA = 1.0 + fT2;
float fB = fT2 * afV[2];
float fC = 1.0 + fB * afV[2];
if ( bIdentity ) {
kL[0][0] = 1.0;
kL[0][1] = kL[1][0] = 0.0;
kL[0][2] = kL[2][0] = 0.0;
kL[1][1] = fA;
kL[1][2] = kL[2][1] = fB;
kL[2][2] = fC;
} else {
for (int iRow = 0; iRow < 3; iRow++) {
float fTmp0 = kL[iRow][1];
float fTmp1 = kL[iRow][2];
kL[iRow][1] = fA * fTmp0 + fB * fTmp1;
kL[iRow][2] = fB * fTmp0 + fC * fTmp1;
}
}
}
}
//----------------------------------------------------------------------------
void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL,
Matrix3& kR) {
float fT11 = kA[0][1] * kA[0][1] + kA[1][1] * kA[1][1];
float fT22 = kA[1][2] * kA[1][2] + kA[2][2] * kA[2][2];
float fT12 = kA[1][1] * kA[1][2];
float fTrace = fT11 + fT22;
float fDiff = fT11 - fT22;
float fDiscr = sqrt(fDiff * fDiff + 4.0 * fT12 * fT12);
float fRoot1 = 0.5 * (fTrace + fDiscr);
float fRoot2 = 0.5 * (fTrace - fDiscr);
// adjust right
float fY = kA[0][0] - (G3D::abs(fRoot1 - fT22) <=
G3D::abs(fRoot2 - fT22) ? fRoot1 : fRoot2);
float fZ = kA[0][1];
float fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ);
float fSin = fZ * fInvLength;
float fCos = -fY * fInvLength;
float fTmp0 = kA[0][0];
float fTmp1 = kA[0][1];
kA[0][0] = fCos * fTmp0 - fSin * fTmp1;
kA[0][1] = fSin * fTmp0 + fCos * fTmp1;
kA[1][0] = -fSin * kA[1][1];
kA[1][1] *= fCos;
int iRow;
for (iRow = 0; iRow < 3; iRow++) {
fTmp0 = kR[0][iRow];
fTmp1 = kR[1][iRow];
kR[0][iRow] = fCos * fTmp0 - fSin * fTmp1;
kR[1][iRow] = fSin * fTmp0 + fCos * fTmp1;
}
// adjust left
fY = kA[0][0];
fZ = kA[1][0];
fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ);
fSin = fZ * fInvLength;
fCos = -fY * fInvLength;
kA[0][0] = fCos * kA[0][0] - fSin * kA[1][0];
fTmp0 = kA[0][1];
fTmp1 = kA[1][1];
kA[0][1] = fCos * fTmp0 - fSin * fTmp1;
kA[1][1] = fSin * fTmp0 + fCos * fTmp1;
kA[0][2] = -fSin * kA[1][2];
kA[1][2] *= fCos;
int iCol;
for (iCol = 0; iCol < 3; iCol++) {
fTmp0 = kL[iCol][0];
fTmp1 = kL[iCol][1];
kL[iCol][0] = fCos * fTmp0 - fSin * fTmp1;
kL[iCol][1] = fSin * fTmp0 + fCos * fTmp1;
}
// adjust right
fY = kA[0][1];
fZ = kA[0][2];
fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ);
fSin = fZ * fInvLength;
fCos = -fY * fInvLength;
kA[0][1] = fCos * kA[0][1] - fSin * kA[0][2];
fTmp0 = kA[1][1];
fTmp1 = kA[1][2];
kA[1][1] = fCos * fTmp0 - fSin * fTmp1;
kA[1][2] = fSin * fTmp0 + fCos * fTmp1;
kA[2][1] = -fSin * kA[2][2];
kA[2][2] *= fCos;
for (iRow = 0; iRow < 3; iRow++) {
fTmp0 = kR[1][iRow];
fTmp1 = kR[2][iRow];
kR[1][iRow] = fCos * fTmp0 - fSin * fTmp1;
kR[2][iRow] = fSin * fTmp0 + fCos * fTmp1;
}
// adjust left
fY = kA[1][1];
fZ = kA[2][1];
fInvLength = 1.0 / sqrt(fY * fY + fZ * fZ);
fSin = fZ * fInvLength;
fCos = -fY * fInvLength;
kA[1][1] = fCos * kA[1][1] - fSin * kA[2][1];
fTmp0 = kA[1][2];
fTmp1 = kA[2][2];
kA[1][2] = fCos * fTmp0 - fSin * fTmp1;
kA[2][2] = fSin * fTmp0 + fCos * fTmp1;
for (iCol = 0; iCol < 3; iCol++) {
fTmp0 = kL[iCol][1];
fTmp1 = kL[iCol][2];
kL[iCol][1] = fCos * fTmp0 - fSin * fTmp1;
kL[iCol][2] = fSin * fTmp0 + fCos * fTmp1;
}
}
//----------------------------------------------------------------------------
void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS,
Matrix3& kR) const {
int iRow, iCol;
Matrix3 kA = *this;
bidiagonalize(kA, kL, kR);
for (int i = 0; i < ms_iSvdMaxIterations; i++) {
float fTmp, fTmp0, fTmp1;
float fSin0, fCos0, fTan0;
float fSin1, fCos1, fTan1;
bool bTest1 = (G3D::abs(kA[0][1]) <=
ms_fSvdEpsilon * (G3D::abs(kA[0][0]) + G3D::abs(kA[1][1])));
bool bTest2 = (G3D::abs(kA[1][2]) <=
ms_fSvdEpsilon * (G3D::abs(kA[1][1]) + G3D::abs(kA[2][2])));
if ( bTest1 ) {
if ( bTest2 ) {
kS[0] = kA[0][0];
kS[1] = kA[1][1];
kS[2] = kA[2][2];
break;
} else {
// 2x2 closed form factorization
fTmp = (kA[1][1] * kA[1][1] - kA[2][2] * kA[2][2] +
kA[1][2] * kA[1][2]) / (kA[1][2] * kA[2][2]);
fTan0 = 0.5 * (fTmp + sqrt(fTmp * fTmp + 4.0));
fCos0 = 1.0 / sqrt(1.0 + fTan0 * fTan0);
fSin0 = fTan0 * fCos0;
for (iCol = 0; iCol < 3; iCol++) {
fTmp0 = kL[iCol][1];
fTmp1 = kL[iCol][2];
kL[iCol][1] = fCos0 * fTmp0 - fSin0 * fTmp1;
kL[iCol][2] = fSin0 * fTmp0 + fCos0 * fTmp1;
}
fTan1 = (kA[1][2] - kA[2][2] * fTan0) / kA[1][1];
fCos1 = 1.0 / sqrt(1.0 + fTan1 * fTan1);
fSin1 = -fTan1 * fCos1;
for (iRow = 0; iRow < 3; iRow++) {
fTmp0 = kR[1][iRow];
fTmp1 = kR[2][iRow];
kR[1][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
kR[2][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
}
kS[0] = kA[0][0];
kS[1] = fCos0 * fCos1 * kA[1][1] -
fSin1 * (fCos0 * kA[1][2] - fSin0 * kA[2][2]);
kS[2] = fSin0 * fSin1 * kA[1][1] +
fCos1 * (fSin0 * kA[1][2] + fCos0 * kA[2][2]);
break;
}
} else {
if ( bTest2 ) {
// 2x2 closed form factorization
fTmp = (kA[0][0] * kA[0][0] + kA[1][1] * kA[1][1] -
kA[0][1] * kA[0][1]) / (kA[0][1] * kA[1][1]);
fTan0 = 0.5 * ( -fTmp + sqrt(fTmp * fTmp + 4.0));
fCos0 = 1.0 / sqrt(1.0 + fTan0 * fTan0);
fSin0 = fTan0 * fCos0;
for (iCol = 0; iCol < 3; iCol++) {
fTmp0 = kL[iCol][0];
fTmp1 = kL[iCol][1];
kL[iCol][0] = fCos0 * fTmp0 - fSin0 * fTmp1;
kL[iCol][1] = fSin0 * fTmp0 + fCos0 * fTmp1;
}
fTan1 = (kA[0][1] - kA[1][1] * fTan0) / kA[0][0];
fCos1 = 1.0 / sqrt(1.0 + fTan1 * fTan1);
fSin1 = -fTan1 * fCos1;
for (iRow = 0; iRow < 3; iRow++) {
fTmp0 = kR[0][iRow];
fTmp1 = kR[1][iRow];
kR[0][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
kR[1][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
}
kS[0] = fCos0 * fCos1 * kA[0][0] -
fSin1 * (fCos0 * kA[0][1] - fSin0 * kA[1][1]);
kS[1] = fSin0 * fSin1 * kA[0][0] +
fCos1 * (fSin0 * kA[0][1] + fCos0 * kA[1][1]);
kS[2] = kA[2][2];
break;
} else {
golubKahanStep(kA, kL, kR);
}
}
}
// positize diagonal
for (iRow = 0; iRow < 3; iRow++) {
if ( kS[iRow] < 0.0 ) {
kS[iRow] = -kS[iRow];
for (iCol = 0; iCol < 3; iCol++)
kR[iRow][iCol] = -kR[iRow][iCol];
}
}
}
//----------------------------------------------------------------------------
void Matrix3::singularValueComposition (const Matrix3& kL,
const Vector3& kS, const Matrix3& kR) {
int iRow, iCol;
Matrix3 kTmp;
// product S*R
for (iRow = 0; iRow < 3; iRow++) {
for (iCol = 0; iCol < 3; iCol++)
kTmp[iRow][iCol] = kS[iRow] * kR[iRow][iCol];
}
// product L*S*R
for (iRow = 0; iRow < 3; iRow++) {
for (iCol = 0; iCol < 3; iCol++) {
elt[iRow][iCol] = 0.0;
for (int iMid = 0; iMid < 3; iMid++)
elt[iRow][iCol] += kL[iRow][iMid] * kTmp[iMid][iCol];
}
}
}
//----------------------------------------------------------------------------
void Matrix3::orthonormalize () {
// Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
// M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
//
// q0 = m0/|m0|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
//
// where |V| indicates length of vector V and A*B indicates dot
// product of vectors A and B.
// compute q0
float fInvLength = 1.0 / sqrt(elt[0][0] * elt[0][0]
+ elt[1][0] * elt[1][0] +
elt[2][0] * elt[2][0]);
elt[0][0] *= fInvLength;
elt[1][0] *= fInvLength;
elt[2][0] *= fInvLength;
// compute q1
float fDot0 =
elt[0][0] * elt[0][1] +
elt[1][0] * elt[1][1] +
elt[2][0] * elt[2][1];
elt[0][1] -= fDot0 * elt[0][0];
elt[1][1] -= fDot0 * elt[1][0];
elt[2][1] -= fDot0 * elt[2][0];
fInvLength = 1.0 / sqrt(elt[0][1] * elt[0][1] +
elt[1][1] * elt[1][1] +
elt[2][1] * elt[2][1]);
elt[0][1] *= fInvLength;
elt[1][1] *= fInvLength;
elt[2][1] *= fInvLength;
// compute q2
float fDot1 =
elt[0][1] * elt[0][2] +
elt[1][1] * elt[1][2] +
elt[2][1] * elt[2][2];
fDot0 =
elt[0][0] * elt[0][2] +
elt[1][0] * elt[1][2] +
elt[2][0] * elt[2][2];
elt[0][2] -= fDot0 * elt[0][0] + fDot1 * elt[0][1];
elt[1][2] -= fDot0 * elt[1][0] + fDot1 * elt[1][1];
elt[2][2] -= fDot0 * elt[2][0] + fDot1 * elt[2][1];
fInvLength = 1.0 / sqrt(elt[0][2] * elt[0][2] +
elt[1][2] * elt[1][2] +
elt[2][2] * elt[2][2]);
elt[0][2] *= fInvLength;
elt[1][2] *= fInvLength;
elt[2][2] *= fInvLength;
}
//----------------------------------------------------------------------------
void Matrix3::qDUDecomposition (Matrix3& kQ,
Vector3& kD, Vector3& kU) const {
// Factor M = QR = QDU where Q is orthogonal, D is diagonal,
// and U is upper triangular with ones on its diagonal. Algorithm uses
// Gram-Schmidt orthogonalization (the QR algorithm).
//
// If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
//
// q0 = m0/|m0|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
//
// where |V| indicates length of vector V and A*B indicates dot
// product of vectors A and B. The matrix R has entries
//
// r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
// r10 = 0 r11 = q1*m1 r12 = q1*m2
// r20 = 0 r21 = 0 r22 = q2*m2
//
// so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
// u02 = r02/r00, and u12 = r12/r11.
// Q = rotation
// D = scaling
// U = shear
// D stores the three diagonal entries r00, r11, r22
// U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
// build orthogonal matrix Q
float fInvLength = 1.0 / sqrt(elt[0][0] * elt[0][0]
+ elt[1][0] * elt[1][0] +
elt[2][0] * elt[2][0]);
kQ[0][0] = elt[0][0] * fInvLength;
kQ[1][0] = elt[1][0] * fInvLength;
kQ[2][0] = elt[2][0] * fInvLength;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -