G3D Innovation Engine

I promised Bill Baxter we'd add the following code soon.

Hi Morgan,

Here's the polar decomposition code I wrote that I'm using with G3D.
Excuse all the typedefs and everything basically being defined inline
(I have it all in a header right now).
I might be using some operators that I added locally in my version.
But I think these should be in the trunk version now too, since I
think you said you added all the ones I suggested.

Some of the constants could probably be better tweaked to give a
marginal performance gain, but it seems to be working pretty well in
my code.
You may license it however G3D is licensed.

--bb

-----------------------------------
typedef Vector2 float2;
typedef Vector3 float3;
typedef Vector4 float4;
typedef Matrix3 float3x3;
typedef Matrix4 float4x4;

inline float DiffOneNorm(const float3x3& x, const float3x3& y)
{
float oneNorm = 0;
for (int c=0; c<3; ++c) {
float f = ::fabs(x[0][c] - y[0][c]) + ::fabs(x[1][c]-y[1][c])
+ ::fabs(x[2][c]-y[2][c]);
if (f>oneNorm) oneNorm = f;
}
return oneNorm;
}

/// the one norm of a matrix is the max column abs sum
inline float OneNorm(const float3x3& x)
{
float oneNorm = 0;
for (int c=0; c<3; ++c) {
float f = ::fabs(x[0][c])+::fabs(x[1][c])+::fabs(x[2][c]);
if (f>oneNorm) oneNorm = f;
}
return oneNorm;
}
/// the infinity norm of a matrix is the max row abs sum
inline float InfNorm(const float3x3& x)
{
float infNorm = 0;
for (int r=0; r<3; ++r) {
float f = ::fabs(x[r][0])+::fabs(x[r][1])+::fabs(x[r][2]);
if (f>infNorm) infNorm = f;
}
return infNorm;
}
inline float OneNormTimesInfNorm(const float3x3& x)
{
return OneNorm(x)*InfNorm(x);
}

inline float3x3& operator*=(float3x3& x, float s)
{
float* xp = x[0];
for (int i=0; i<9; ++i) {
xp[i] *= s;
}
return x;
}

/*
Polar decomposition of a matrix. Based on pseudocode from
Nicholas J Higham, "Computing the Polar Decomposition -- with
Applications
Siam Journal of Science and Statistical Computing, Vol 7, No. 4,
October 1986.

Decomposes A into R*S, where R is orthogonal and S is symmetric.

Ken Shoemake's "Matrix animation and polar decomposition"
in Proceedings of the conference on Graphics interface '92
seems to be better known in the world of graphics, but Higham's version
uses a scaling constant that can lead to faster convergence than
Shoemake's
when the initial matrix is far from orthogonal.
*/
inline void polarDecomposition(const float3x3& A, float3x3& R, float3x3&
S)
{
float3x3 X = A;
float3x3 Xit;
float3x3 tmp;
int iter = 0;
const int MAX_ITERS = 100;

const double eps = 50*std::numeric_limits<float>::epsilon();
const float BigEps = 50*eps;

tmp = X.inverse();
float3x3::transpose(tmp, Xit);

// Higham suggests using OneNorm(Xit-X) < eps * OneNorm(X)
// as the convergence criterion, but OneNorm(X) should quickly
// settle down to something between 1 and 1.7, so just comparing
// with eps seems sufficient.
double resid = DiffOneNorm(X,Xit);
while (resid > eps && iter < MAX_ITERS)
{
tmp = X.inverse();
float3x3::transpose(tmp, Xit);

if (resid < BigEps) {
// close enough use simple iteration
X += Xit;
X *= 0.5f;
}
else {
// not close to convergence, compute acceleration factor
float gamma = sqrt( sqrt(
(OneNorm(Xit)*InfNorm(Xit))/(OneNorm(X)*InfNorm(X)) ) );
X *= 0.5f*gamma;
tmp = Xit;
tmp *= 0.5f/gamma;
X += tmp;
}

resid = DiffOneNorm(X,Xit);
iter++;
}
R = X;
float3x3::transpose(R, tmp);
S = tmp * A;
// S := (S + S^t)/2 one more time to make sure it is symmetric
float3x3::transpose(S,tmp);
S += tmp;
S *= 0.5f;
#ifdef _DEBUG
// Check iter limit
assert(iter < MAX_ITERS);
// Check A = R*S
tmp = R*S;
resid = DiffOneNorm(tmp, A);
assert(resid<eps);
// Check R is orthogonal
tmp = R*R.transpose();
resid = DiffOneNorm(tmp, float3x3::identity());
assert(resid<eps);
// Check that S is symmetric
tmp = S.transpose();
resid = DiffOneNorm(tmp, S);
assert(resid<eps);
#endif
}

// ------------ some tests --------------
double frobeniusNormDiff(const float3x3& a, const float3x3& b)
{
double d = 0;
for (int i=0; i<9; ++i) {
double d0 = ((const float*)(a))[i] - ((const float*)(b))[i];
d += d0 * d0;
}
return sqrt(d);
}

void polarDecompositionTests()
{
// TEST PURE ROTATION
float3x3 A0 = float3x3::fromAxisAngle(float3(1,-2,3).unit(),
1.23f);
float3x3 R1,S1;
polarDecomposition(A0,R1,S1);
float3x3 A1 = R1 * S1;
double fn;
fn = frobeniusNormDiff(A0,A1); // check that it decomposes A0
assert(fn < 1e-4);
fn = R1.determinant();
assert(fabs(1 - fn) < 1e-4); // R is special orthagonal if
initial det > 0
fn =
frobeniusNormDiff(float3x3::identity(),R1*R1.transpose()); // check R
is orthogonal
assert(fn < 1e-4);
fn = frobeniusNormDiff(S1,S1.transpose()); // check S is
symmetric
assert(fn < 1e-4);
fn = frobeniusNormDiff(S1,float3x3::identity()); // check
S is identity in this case
assert(fn < 1e-4);

// TEST GENERAL MATRIX det>0
A0 = float3x3::fromAxisAngle(float3(.1f,-1,.3f).unit(), 2.3f);
A0 *= float3x3(
.1f ,-.2f ,.3f,
.3f ,.2f ,.1f,
-.1f ,.2f ,.4f);
assert(A0.determinant() > 0);
polarDecomposition(A0,R1,S1);
A1 = R1 * S1;
fn = frobeniusNormDiff(A0,A1); // check that it decomposes A0
assert(fn < 1e-4);
fn = R1.determinant();
assert(fabs(1 - fn) < 1e-4); // R is special orthagonal if
initial det > 0
fn =
frobeniusNormDiff(float3x3::identity(),R1*R1.transpose()); // check R
is orthogonal
assert(fn < 1e-4);
fn = frobeniusNormDiff(S1,S1.transpose()); // check S is
symmetric
assert(fn < 1e-4);

// TEST GENERAL MATRIX det<0
A0 = float3x3::fromAxisAngle(float3(.1f,-1,.3f).unit(), 2.3f);
A0 *= float3x3(
-.1f ,-.2f ,.3f,
-.3f ,.2f ,.1f,
+.1f ,.2f ,.4f);
assert(A0.determinant() < 0);
polarDecomposition(A0,R1,S1);
fn = R1.determinant();
assert(fabs(-1 - fn) < 1e-4); // Neg det on A0 yields R
that reflects
A1 = R1 * S1;
fn = frobeniusNormDiff(A0,A1); // check that it decomposes A0
assert(fn < 1e-4);
fn =
frobeniusNormDiff(float3x3::identity(),R1*R1.transpose()); // check R
is orthogonal
assert(fn < 1e-4);
fn = frobeniusNormDiff(S1,S1.transpose()); // check S is
symmetric
assert(fn < 1e-4);
}