I had to review some physics code for a client recently. Although the physics model being used was very simple point physics, the code was quite long and the algorithms were not immediately obvious. This tends to happen when code isn't refactored as it is modified and extended.
I made a few suggestions about the code, one of which was to try and wrap the underlying math routines in unit tests. This is a really simple way to catch bugs that can otherwise result in strange behavior, like the ball disappearing or missing collisions. It also lays the foundation for performing code refactoring that can dramatically improve the quality of the code.
As a little test, I decided to write a small physics project to illustrate this in action.
Scope Of The Project
I didn't want to write a full physics engine for this test. I've written one before and it's not a small undertaking. Then I remembered an old Amiga demo I saw a million years ago that had a soft-body jelly vector object contained within a spinning cube. No, I can't for the life of me remember the name of the demo. I decided to try to recreate it in C# and then use unit tests and code refactoring to clean up the code.
Cleaning Up The Code
The idea was simple. Write the most flat unstructured code and get it up an running. This code base represents the 'mess' that has to be cleaned up and refactored. Even basic code refactoring can have a dramatic effect on this code. Have a look below.
// Apply spring force
for (int l = 0; l < _jellyLines.Length / 2; l += 1)
{
int line = l * 2;
int vertex1 = _jellyLines[line] * 3;
int vertex2 = _jellyLines[line + 1] * 3;
// Get Spring length
float x1 = _jelly_os[vertex1];
float y1 = _jelly_os[vertex1 + 1];
float z1 = _jelly_os[vertex1 + 2];
float x2 = _jelly_os[vertex2];
float y2 = _jelly_os[vertex2 + 1];
float z2 = _jelly_os[vertex2 + 2];
float lineRestLength = _jellyLineLengths[l];
float lineLength = (float)Math.Sqrt((z2 - z1) * (z2 - z1) + (y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1));
// Get Relative Velocity of both end points
float rvx = _jelly_velocity[vertex2] - _jelly_velocity[vertex1];
float rvy = _jelly_velocity[vertex2 + 1] - _jelly_velocity[vertex1 + 1];
float rvz = _jelly_velocity[vertex2 + 2] - _jelly_velocity[vertex1 + 2];
float relativeVelocity = (float)Math.Sqrt(rvz * rvz + rvy * rvy + rvx * rvx);
// Calculate Spring Force
float force = _springK * (lineLength - lineRestLength) - _springB * relativeVelocity;
// Get Spring Vector
float vx = _jelly_os[vertex2] - _jelly_os[vertex1];
float vy = _jelly_os[vertex2 + 1] - _jelly_os[vertex1 + 1];
float vz = _jelly_os[vertex2 + 2] - _jelly_os[vertex1 + 2];
// Normalise
float vl = (float)Math.Sqrt(vz * vz + vy * vy + vx * vx);
vx = vl != 0.0f ? vx / vl : 0.0f;
vy = vl != 0.0f ? vy / vl : 0.0f;
vz = vl != 0.0f ? vz / vl : 0.0f;
// Apply force to both ends of the Spring
_jelly_force[vertex1] += vx * -force;
_jelly_force[vertex1 + 1] += vy * -force;
_jelly_force[vertex1 + 2] += vz * -force;
_jelly_force[vertex2] += vx * force;
_jelly_force[vertex2 + 1] += vy * force;
_jelly_force[vertex2 + 2] += vz * force;
}
I've made no attempt to use any OO design at all in the above code. It iterates over the lines/springs making up our soft-body jelly object and applies the spring forces to both ends of the spring. It stinks of many of the classic bad code smells. After a afternoon of refactoring the code for the entire project, the above code ended up as follows.
public void ApplyForceToEndPoints(PointMass[] points)
{
Vector3 springVector = GetSpringVector(points);
Vector3 relativeVelocityVector = GetEndPointsRelativeVelocityVector(points);
Vector3 springForceVector = GetSpringForceVector(springVector, relativeVelocityVector);
points[Point1].Force -= springForceVector;
points[Point2].Force += springForceVector;
}
For a start the refactored code above is a lot smaller. This is because duplicate functionality has been extracted along with operators and methods and classes. Notice also that this method is a class method and operates on only one spring and not an array of springs. Finally, the need for comments is gone thanks to the new refactored method names.
Another example is shown below.
// Integrate
for (int v = 0; v < _jelly_os.Length / 3; v += 1)
{
int vertex = v * 3;
// Assume mass is 1.0, apply acceleration
_jelly_velocity[vertex] += _jelly_force[vertex] * interval / 1000;
_jelly_velocity[vertex + 1] += _jelly_force[vertex + 1] * interval / 1000;
_jelly_velocity[vertex + 2] += _jelly_force[vertex + 2] * interval / 1000;
// Apply velocity
_jelly_os[vertex] += _jelly_velocity[vertex];
_jelly_os[vertex + 1] += _jelly_velocity[vertex + 1];
_jelly_os[vertex + 2] += _jelly_velocity[vertex + 2];
}
// Clip vertices
for (int v = 0; v < _jelly_os.Length / 3; v += 1)
{
int vertex = v * 3;
float x = _jelly_os[vertex];
float y = _jelly_os[vertex + 1];
float z = _jelly_os[vertex + 2];
for (int p = 0; p < _cubePlanes_ws.Length / 4; p++)
{
int plane = p * 4;
float px = _cubePlanes_ws[plane];
float py = _cubePlanes_ws[plane + 1];
float pz = _cubePlanes_ws[plane + 2];
float pd = _cubePlanes_ws[plane + 3];
float clipD = px * x + py * y + pz * z;
if (clipD < pd)
{
x += px * -(clipD - pd);
y += py * -(clipD - pd);
z += pz * -(clipD - pd);
// Remove this vector component from the velocity vector
float velocityD = px * _jelly_velocity[vertex] + py * _jelly_velocity[vertex + 1] + pz * _jelly_velocity[vertex + 2];
_jelly_velocity[vertex] -= px * velocityD;
_jelly_velocity[vertex + 1] -= py * velocityD;
_jelly_velocity[vertex + 2] -= pz * velocityD;
}
}
_jelly_os[vertex] = x;
_jelly_os[vertex + 1] = y;
_jelly_os[vertex + 2] = z;
}
The above code uses Euler integration to move the soft-body jelly object forward in time. It then clips the vertices or points of the soft-body against the planes of the cube in which it is contained. Both operations occur within for loops. The same bad code smells from earlier are also evident here.
Both of the above for loops iterate over the points of the soft-body. That's where I started when I began to refactor the code. When I was finished, I ended up with a PointMass class that housed most of the above functionality.
public class PointMass
{
public Vector3 Point { get; set; }
public Vector3 Velocity { get; set; }
public Vector3 Force { get; set; }
public float Mass { get; set; }
public PointMass()
{
Point = new Vector3();
Velocity = new Vector3();
Force = new Vector3();
Mass = 1;
}
public void Integrate(int intervalInMilliseconds)
{
Velocity += Force * (((float)intervalInMilliseconds / 1000) / Mass);
Point += Velocity;
}
public void ClipToPlanes(PipelinePlane[] planes)
{
foreach(PipelinePlane plane in planes)
ClipPointAndVelocityToPlane(plane.WorldPlane);
}
private void ClipPointAndVelocityToPlane(Plane plane)
{
float distance;
if (!plane.IsPointInfront( Point, out distance))
{
Point = plane.Clip(Point);
RemoveVectorComponentFromVelocity(plane.normal);
}
}
private void RemoveVectorComponentFromVelocity(Vector3 vector)
{
float velocityDistanceAlongVector = vector.DotProduct(Velocity);
Velocity -= vector * velocityDistanceAlongVector;
}
}
The above code is much easier to read and is easy to debug and maintain. Again, the need for comments is largely gone.
Unit Tests
The unit test project contains 21 tests covering most of the maths routines. These tests are extremely simple and are by no means complete. But if the project is developed further, more tests can be added easily. Likewise, if bugs are found, new tests can be added to confirm the fix and ensure it doesn't pop up again.
Another benefit of having unit tests in place appears when optimizations are required. Take the Length method of the 3D Vector object as shown below.
public float Length()
{
return (float)System.Math.Sqrt(z * z + y * y + x * x);
}
The Length method above has been implemented using the System.Math.Sqrt method. While the square root operation is accurate, it is also usually a costly one, so developers often replace the square root operation with something more efficient but less accurate. The degree to which we are willing to sacrifice accuracy for an increase in efficiency can be encoded in our unit tests by specifying a tolerance value as shown below.
[Test]
public void VectorLengthTest()
{
Vector3 vector = new Vector3(221, 132, 40);
float vectorTolerance = 0.00001f;
float expectedLength = (float)System.Math.Sqrt(result.x*result.x + result.y*result.y + result.z*result.z);
Assert.AreEqual(expectedLength, vector.Length(), vectorTolerance, "Vector Length is not within acceptable tolerance");
}
This allows us to set the boundaries within which to optimize. I used this approach before when writing a custom software version of the Mascot Capsule 3D API while at Upstart Games. I recorded the output of the math routines from Mascot Capsule and set up a series of tests with a specified tolerance. I used these tests to ensure my own pipeline produced the same values within the specified tolerance. This allowed me to emulate and optimize at the same time without breaking anything.
Conclusion
Simple code refactoring and unit testing can be powerful tools to help developers take control of their code no matter what type of software they develop. You can download the full source (including unit tests) for the project and the binaries below. Again, these are not perfect examples but they do demonstrate some of the benefits of using refactoring and unit tests.
JellyVector.zip (54.08 kb)
JellyVector.bin.zip (11.76 kb)
[Update: The Amiga demo I referred to earlier was called Krest Mass Leftovers by Anarchy. It was released in 1992.]