Linear Algebra Package

These functions are used to numerically calculate solutions to linear systems and find eigenvalues of symmetric matrices. For details, see Mathematics for 3D Game Programming & Computer Graphics, Chapter 14.

SolveLinearSystem

bool SolveLinearSystem(int n, float *m, float *r)
{
	float *rowNormalizer = new float[n];
	bool result = false;
	
	// Calculate a normalizer for each row
	for (int i = 0; i < n; i++)
	{
		const float *entry = m + i;
		float maxvalue = 0.0F;
		
		for (int j = 0; j < n; j++)
		{
			float value = fabs(*entry);
			if (value > maxvalue) maxvalue = value;
			entry += n;
		}
		
		if (maxvalue == 0.0F) goto exit; // Singular
		rowNormalizer[i] = 1.0F / maxvalue;
	}
	
	// Perform elimination one column at a time
	for (int j = 0; j < n - 1; j++)
	{
		// Find pivot element
		int pivotRow = -1;
		float maxvalue = 0.0F;
		for (int i = j; i < n; i++)
		{
			float p = fabs(m[j * n + i]) * rowNormalizer[i];
			if (p > maxvalue)
			{
				maxvalue = p;
				pivotRow = i;
			}
		}
		
		if (pivotRow != j)
		{
			if (pivotRow == -1) goto exit; // Singular
			
			// Exchange rows
			for (int k = 0; k < n; k++)
			{
				float temp = m[k * n + j];
				m[k * n + j] = m[k * n + pivotRow];
				m[k * n + pivotRow] = temp;
			}
			
			float temp = r[j];
			r[j] = r[pivotRow];
			r[pivotRow] = temp;
			
			rowNormalizer[pivotRow] = rowNormalizer[j];
		}
		
		float denom = 1.0F / m[j * n + j];
		for (int i = j + 1; i < n; i++)
		{
			float factor = m[j * n + i] * denom;
			r[i] -= r[j] * factor;
			for (int k = 0; k < n; k++) m[k * n + i] -= m[k * n + j] * factor;
		}
	}
	
	// Perform backward substitution
	for (int i = n - 1; i >= 0; i--)
	{
		float sum = r[i];
		for (int k = i + 1; k < n; k++) sum -= m[k * n + i] * r[k];
		r[i] = sum / m[i * n + i];
	}

	result = true;
	
	exit:
	delete[] rowNormalizer;
	return (result);
}

LUDecompose

bool LUDecompose(int n, float *m,
	unsigned short *index, float *detSign)
{
	float *rowNormalizer = new float[n];
	float exchangeParity = 1.0F;
	bool result = false;
	
	// Calculate a normalizer for each row
	for (int i = 0; i < n; i++)
	{
		const float *entry = m + i;
		float maxvalue = 0.0F;
		
		for (int j = 0; j < n; j++)
		{
			float value = fabs(*entry);
			if (value > maxvalue) maxvalue = value;
			entry += n;
		}
		
		if (maxvalue == 0.0F) goto exit; // Singular
		rowNormalizer[i] = 1.0F / maxvalue;
		index[i] = i;
	}
	
	// Perform decomposition
	for (int j = 0; j < n; j++)
	{
		for (int i = 1; i < j; i++)
		{
			// Evaluate Equation (14.15)
			float sum = m[j * n + i];
			for (int k = 0; k < i; k++) sum -= m[k * n + i] * m[j * n + k];
			m[j * n + i] = sum;
		}
		
		// Find pivot element
		int pivotRow = -1;
		float maxvalue = 0.0F;
		for (int i = j; i < n; i++)
		{
			// Evaluate Equation (14.17)
			float sum = m[j * n + i];
			for (int k = 0; k < j; k++) sum -= m[k * n + i] * m[j * n + k];
			m[j * n + i] = sum;
			
			sum = fabs(sum) * rowNormalizer[i];
			if (sum > maxvalue)
			{
				maxvalue = sum;
				pivotRow = i;
			}
		}
		
		if (pivotRow != j)
		{
			if (pivotRow == -1) goto exit; // Singular
			
			// Exchange rows
			for (int k = 0; k < n; k++)
			{
				float temp = m[k * n + j];
				m[k * n + j] = m[k * n + pivotRow];
				m[k * n + pivotRow] = temp;
			}
			
			unsigned short temp = index[j];
			index[j] = index[pivotRow];
			index[pivotRow] = temp;
			
			rowNormalizer[pivotRow] = rowNormalizer[j];
			exchangeParity = -exchangeParity;
		}
		
		// Divide by pivot element
		if (j != n - 1)
		{
			float denom = 1.0F / m[j * n + j];
			for (int i = j + 1; i < n; i++) m[j * n + i] *= denom;
		}
	}
	
	if (detSign) *detSign = exchangeParity;
	result = true;

	exit:
	delete[] rowNormalizer;
	return (result);
}

LUBacksubstitute

void LUBacksubstitute(int n, const float *d,
	const unsigned short *index, const float *r, float *x)
{
	for (int i = 0; i < n; i++) x[i] = r[index[i]];
	
	// Perform forward substitution for Ly = r
	for (int i = 0; i < n; i++)
	{
		float sum = x[i];
		for (int k = 0; k < i; k++) sum -= d[k * n + i] * x[k];
		x[i] = sum;
	}
	
	// Perform backward substitution for Ux = y
	for (int i = n - 1; i >= 0; i--)
	{
		float sum = x[i];
		for (int k = i + 1; k < n; k++) sum -= d[k * n + i] * x[k];
		x[i] = sum / d[i * n + i];
	}
}

LURefineSolution

void LURefineSolution(int n, const float *m,
	const float *d, const unsigned short *index,
	const float *r, float *x)
{
	float *t = new float[n];
	
	for (int i = 0; i < n; i++)
	{
		double q = -r[i];
		for (int k = 0; k < n; k++) q += m[k * n + i] * x[k];
		t[i] = (float) q;
	}
	
	LUBacksubstitute(n, d, index, t, t);
	for (int i = 0; i < n; i++) x[i] -= t[i];
	
	delete[] t;
}

SolveTridiagonalSystem

void SolveTridiagonalSystem(int n,
	const float *a, const float *b, const float *c,
	const float *r, float *x)
{
	// Allocate temporary storage for c[i]/beta[i]
	float *t = new float[n - 1];

	float recipBeta = 1.0F / b[0];
	x[0] = r[0] * recipBeta;

	for (int i = 1; i < n; i++)
	{
		t[i - 1] = c[i - 1] * recipBeta; 
		recipBeta = 1.0F / (b[i] - a[i] * t[i - 1]);
		x[i] = (r[i] - a[i] * x[i - 1]) * recipBeta;
	}

	for (int i = n - 2; i >= 0; i--) x[i] -= t[i] * x[i + 1];

	delete[] t;
}

CalculateEigensystem

const float epsilon = 1.0e-10F;
const int maxSweeps = 32;


struct Matrix3D
{
	float n[3][3];

	float& operator()(int i, int j)
	{
		return (n[j][i]);
	}

	const float& operator()(int i, int j) const
	{
		return (n[j][i]);
	}

	void SetIdentity(void)
	{
		n[0][0] = n[1][1] = n[2][2] = 1.0F;
		n[0][1] = n[0][2] = n[1][0] = n[1][2] = n[2][0] = n[2][1] = 0.0F;
	}
};


void CalculateEigensystem(const Matrix3D& m, float *lambda, Matrix3D& r)
{
	float m11 = m(0,0);
	float m12 = m(0,1);
	float m13 = m(0,2);
	float m22 = m(1,1);
	float m23 = m(1,2);
	float m33 = m(2,2);
	
	r.SetIdentity();
	for (int a = 0; a < maxSweeps; a++)
	{
		// Exit if off-diagonal entries small enough
		if ((Fabs(m12) < epsilon) && (Fabs(m13) < epsilon) &&
			(Fabs(m23) < epsilon)) break;
		
		// Annihilate (1,2) entry
		if (m12 != 0.0F)
		{
			float u = (m22 - m11) * 0.5F / m12;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ?
				((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
			float c = 1.0F / sqrt(t * t + 1.0F);
			float s = c * t;
			
			m11 -= t * m12;
			m22 += t * m12;
			m12 = 0.0F;
			
			float temp = c * m13 - s * m23;
			m23 = s * m13 + c * m23;
			m13 = temp;
			
			for (int i = 0; i < 3; i++)
			{
				float temp = c * r(i,0) - s * r(i,1);
				r(i,1) = s * r(i,0) + c * r(i,1);
				r(i,0) = temp;
			}
		}
		
		// Annihilate (1,3) entry
		if (m13 != 0.0F)
		{
			float u = (m33 - m11) * 0.5F / m13;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ?
				((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
			float c = 1.0F / sqrt(t * t + 1.0F);
			float s = c * t;
			
			m11 -= t * m13;
			m33 += t * m13;
			m13 = 0.0F;
			
			float temp = c * m12 - s * m23;
			m23 = s * m12 + c * m23;
			m12 = temp;
			
			for (int i = 0; i < 3; i++)
			{
				float temp = c * r(i,0) - s * r(i,2);
				r(i,2) = s * r(i,0) + c * r(i,2);
				r(i,0) = temp;
			}
		}
		
		// Annihilate (2,3) entry
		if (m23 != 0.0F)
		{
			float u = (m33 - m22) * 0.5F / m23;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ?
				((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
			float c = 1.0F / sqrt(t * t + 1.0F);
			float s = c * t;
			
			m22 -= t * m23;
			m33 += t * m23;
			m23 = 0.0F;
			
			float temp = c * m12 - s * m13;
			m13 = s * m12 + c * m13;
			m12 = temp;
			
			for (int i = 0; i < 3; i++)
			{
				float temp = c * r(i,1) - s * r(i,2);
				r(i,2) = s * r(i,1) + c * r(i,2);
				r(i,1) = temp;
			}
		}
	}
	
	lambda[0] = m11;
	lambda[1] = m22;
	lambda[2] = m33;
}

Visit http://www.terathon.com for information about the book and C4 Engine.

I have this book, and Eric has a great approach to several topics that are straight-forward in design.

I'd say this and Eberly's Appendix A in Game Physics are really all you need except for maybe more on fluid dynamics. I recall that Eric touches on it some including wave dynamics.

I don't know about those goto's though

corey

That's a great book, any programmer that's interested in "3d" math should consider looking that book up . That was a some good pieces of code. Thanks for posting this Eric, I learned a thing or two.

Hi --

I just wanted to point out that the above post was actually made by a DevMaster staff member who asked my permission to post some code that's also available on my website (note different username).

About the goto statements... I know a lot of coding philosophies consider them to be instruments of the devil that should never be used in a million years, but they do have their place. If the C language had a way to break out of nested loops, then I don't think the goto statement would be necessary, but right now, it's the most efficient way to do either of the following.

for (...)
{
    for (...)
    {
        ...
        if (some condition) goto done;  // Can't just break here
        ...
    }
}

done:
...

for (...)
{
    ...
    if (some condition) goto done;  // Can't break because we want to skip code after loop
    ...
}

...
// do something else here
...

done:
...

In the code snippets, I use goto for the second case here so that I can jump to the cleanup code before returning. Using some of the more recent features of C (which are unfortunately not yet available in C++), I could have allocated the array of floats that need to be deleted as an auto on the stack (using variable-sized arrays). In C++, I could wrap the allocation of the float array in some kind of local object that would delete the array when the object is destroyed. Either of these would let me return at the points where I used the goto statement, and the compiler would automatically clean up. Is it worth the mess and confusion? Probably not in this case -- I'd prefer to keep it simple.

-- Eric Lengyel

Originally Posted by elengyel

Using some of the more recent features of C (which are unfortunately not yet available in C++), I could have allocated the array of floats that need to be deleted as an auto on the stack (using variable-sized arrays).

And the next spec isn't until, what, 2008? The developers said MSVC++ won't include "all" C99 features unless users persistently ask for them because they're useful or they are already widely used. So, imagine 8.1+ to be the first to carry even more C99 features. Then again, MSVC++ doesn't claim to be a C compiler exactly.

Originally Posted by elengyel

In C++, I could wrap the allocation of the float array in some kind of local object that would delete the array when the object is destroyed. Either of these would let me return at the points where I used the goto statement, and the compiler would automatically clean up. Is it worth the mess and confusion? Probably not in this case -- I'd prefer to keep it simple.

Exactly the reason I didn't complain loudly You have to either duplicate code, duplicate return values or use a more extensive library.

The follow up was unexpected and appreciated!

Maybe one of you writers will do a wave/fluid example also.

Aww you only implemented the easy algorithms for general matrices . The eigenvalue problem for 3x3 matrices can be solved closed form and can handle complex eigenvalues and eigenvectors which your method does not.

Sure, complex eigenvalues are important in mathematics, but seriously, how often does one need to deal with them in a 3D graphics context?

The eigenvalues and eigenvectors of a rotation matrix are complex.

So? What are eigenvalues and eigenvectors actually used for in 3D graphics? As far as I know, primarily in things like finding best-fit OBBs, in which case the eigenvalues are all real since the covariance matrix is symmetric. Another case might be in analyzing real dynamical systems, in which case again the only eigenvectors you really care about are the real ones.