C++ Data Structures for Rigid-Body Physics

As graphics gets pushed more and more onto specialized hardware, programmers are finally getting the spare CPU cycles they need to bring game physics to realistic levels. Unfortunately, as realism increases, so does the complexity of the code. Starting off with the right data structures can greatly simplify the tedious, error-prone task of programming and maintaining a physics engine, and this article offers suggestions for helping to structure your C++ code more efficiently .

Why Use C++ instead of C?

It seems like I spend about 10% of my time writing code and 90% of my time debugging it, so clean code makes my job a lot easier. The C++ programming language offers many nice extensions to C while keeping its clean, flexible syntax. With it I can write clean, optimized code quickly so I can get to the serious business of finding out why it doesn’t work. Here are the main reasons why I prefer C++ over C.

1. Function inlining. It is a common misconception that C++ produces slow, bulky code and is therefore unsuitable for game programming. With function inlining, C++ can produce functions that are as fast as macros. But unlike macros, inline functions have strict type checking and are viewable in a debugger. And let’s face it: if you really need blistering speed, you write it in assembly. For a more detailed comparison of the performance of C and C++, see [5] in the bibliography at the end of this article.

2. Operator overloading. If my house was burning down and I could take only one feature of C++ with me, it would have to be the ability to overload operators. Operator overloading allows you to use the language’s built in operators (+, -, /, etc.) with user defined types as arguments. As an example, let’s look at linear interpolating between two vectors:

const VECTOR c = (1-u)*a + u*b;

The above C++ statement reads as if it was copied directly from a textbook. The fastest way to do the same operation in C is with macros:

VECTOR s, t, c;

vmul( s, a, 1-u );

vmul( t, b, u );

vadd( c, s, t );

What took one line to write in C++, took four lines to write in C, and it’s is not very easy to read. It can get worse. I didn’t fully appreciate C++ until I wrote the physics engine for a racing game in C. The code took ten times as long to write, and when I came back a week later to fix a bug, I could barely follow what I had written.

3. Inheritance. In C, building complex data types can be done by including structures within structures. This makes accessing deep down members a real pain:

VECTOR R1 = body.PreviousState.Frame.Basis[1];

In C++, data types can be built with inheritance, so any member variable or function a base class had the derived class has as well. By using inheritance, you can make the above C statement look like:

VECTOR R1 = body.Basis[1];

Now that I’ve rambled on about how great C++ is, let me start describing some useful data types.

The SCALAR type

In any meaningful physics program, you’ll need to represent decimal numbers. You could explicitly make everything a float or a double, but what would you do if your program requirements change mid-project and you suddenly need double’s instead of float’s, or vice-versa? You’d have to globally replace every declaration in your program. Or what if you’re randomly experiencing floating-point overflows and you need some range checking in debug mode? Then you might have to go through and wrap every arithmetic operation with a macro. In any case, it’s probably not the best idea to hardcode a standard type for your scalar quantities. The SCALAR type (see Listing 1) is there to abstract your scalar quantities so you can easily change its internal representation.

The VECTOR type

A vector is an ordered set of numbers that represents a magnitude and a direction. Vectors can be added, subtracted, multiplied and divided by scalars, and have special operations such as the dot and cross product. They are used to represent almost every non-scalar quantity in physics and math: velocity, acceleration, surface normal, unit direction, etc. Since most of the physics in games is only in three dimensions, the VECTOR type has only x, y and z components. All basic vector operations are implemented as overloaded operators or inline member functions. The VECTOR type will also be the building block for the MATRIX class.

In Listing 1, notice that the members of VECTOR are public. Object-oriented purists may freak out, but having the ability to directly access the components of the vector makes math and physics programming much easier in the long run. Also, you might notice the const keyword permeates the member functions. The const in front of the return type means that the return value cannot be assigned. The const in front of the arguments means that the arguments are not changed within the function. The const after the argument list means that none of the members of the calling object are changed within that function. In my opinion, const should be used liberally since it will generate a compile error on a statement like:

if( (a + b) = c )

{

printf( "Typos are easy to overlook.\n" );

}

Points have pretty much the same operations as vectors, so they are simply typedef’ed as a VECTOR (see Listing 1).

Listing 1.

#include

// A floating point number

//

typedef float SCALAR;

//

// A 3D vector

//

class VECTOR

{

public:

SCALAR x,y,z; //x,y,z coordinates

public:

VECTOR()

: x(0), y(0), z(0)

{}

VECTOR( const SCALAR& a, const SCALAR& b, const SCALAR& c )

: x(a), y(b), z(c)

{}

//index a component

//NOTE: returning a reference allows

//you to assign the indexed element

SCALAR& operator [] ( const long i )

{

return *((&x) + i);

}

//compare

const bool operator == ( const VECTOR& v ) const

{

return (v.x==x && v.y==y && v.z==z);

}

const bool operator != ( const VECTOR& v ) const

{

return !(v == *this);

}

//negate

const VECTOR operator - () const

{

return VECTOR( -x, -y, -z );

}

//assign

const VECTOR& operator = ( const VECTOR& v )

{

x = v.x;

y = v.y;

z = v.z;

return *this;

}

//increment

const VECTOR& operator += ( const VECTOR& v )

{

x+=v.x;

y+=v.y;

z+=v.z;

return *this;

}

//decrement

const VECTOR& operator -= ( const VECTOR& v )

{

x-=v.x;

y-=v.y;

z-=v.z;

return *this;

}

//self-multiply

const VECTOR& operator *= ( const SCALAR& s )

{

x*=s;

y*=s;

z*=s;

return *this;

}

//self-divide

const VECTOR& operator /= ( const SCALAR& s )

{

const SCALAR r = 1 / s;

x *= r;

y *= r;

z *= r;

return *this;

}

//add

const VECTOR operator + ( const VECTOR& v ) const

{

return VECTOR(x + v.x, y + v.y, z + v.z);

}

//subtract

const VECTOR operator - ( const VECTOR& v ) const

{

return VECTOR(x - v.x, y - v.y, z - v.z);

}

//post-multiply by a scalar

const VECTOR operator * ( const SCALAR& s ) const

{

return VECTOR( x*s, y*s, z*s );

}

//pre-multiply by a scalar

friend inline const VECTOR operator * ( const SCALAR& s, const VECTOR& v )

{

return v * s;

}

//divide

const VECTOR operator / (SCALAR s) const

{

s = 1/s;

return VECTOR( s*x, s*y, s*z );

}

//cross product

const VECTOR cross( const VECTOR& v ) const

{

//Davis, Snider, "Introduction to Vector Analysis", p. 44

return VECTOR( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x );

}

//scalar dot product

const SCALAR dot( const VECTOR& v ) const

{

return x*v.x + y*v.y + z*v.z;

}

//length

const SCALAR length() const

{

return (SCALAR)sqrt( (double)this->dot(*this) );

}

//unit vector

const VECTOR unit() const

{

return (*this) / length();

}

//make this a unit vector

void normalize()

{

(*this) /= length();

}

//equal within an error ‘e’

const bool nearlyEquals( const VECTOR& v, const SCALAR e ) const

{

return fabs(x-v.x)

}

};

//

// A 3D position

//

typedef VECTOR POINT;

The MATRIX type

Matrices allow you to perform linear transformations on vectors. In addition to arithmetic, the MATRIX type (see Listing 2) defines matrix-specific operations such as determinant, inverse, and transpose.

A common way to define a matrix is with a 3x3 array of scalars. I don’t like this method for two reasons: it’s hard to read and it’s easy to forget whether you used a [row][column] or [column][row] indexing scheme. For me, it’s more convenient to think of a matrix as an array of three column vectors. This way, all the matrix operations can easily be written as aggregates of vector operations. The only drawback to this is that is uses a [column][row] indexing scheme, instead of the standard [row][column] (alternatively, you can treat a matrix as an array of three row vectors, but the code is slightly less elegant).

Listing 2.

// A 3x3 matrix

//

class MATRIX

{

public:

VECTOR C[3]; //column vectors

public:

MATRIX()

{

//identity matrix

C[0].x = 1;

C[1].y = 1;

C[2].z = 1;

}

MATRIX( const VECTOR& c0, const VECTOR& c1, const VECTOR& c2 )

{

C[0] = c0;

C[1] = c1;

C[2] = c2;

}

//index a column, allow assignment

//NOTE: using this index operator along with the vector index

//gives you M[column][row], not the standard M[row][column]

VECTOR& operator [] ( long i )

{

return C[i];

}

//compare

const bool operator == ( const MATRIX& m ) const

{

return C[0]==m.C[0] && C[1]==m.C[1] && C[2]==m.C[2];

}

const bool operator != ( const MATRIX& m ) const

{

return !(m == *this);

}

//assign

const MATRIX& operator = ( const MATRIX& m )

{

C[0] = m.C[0];

C[1] = m.C[1];

C[2] = m.C[2];

return *this;

}

//increment

const MATRIX& operator +=( const MATRIX& m )

{

C[0] += m.C[0];

C[1] += m.C[1];

C[2] += m.C[2];

return *this;

}

//decrement

const MATRIX& operator -=( const MATRIX& m )

{

C[0] -= m.C[0];

C[1] -= m.C[1];

C[2] -= m.C[2];

return *this;

}

//self-multiply by a scalar

const MATRIX& operator *= ( const SCALAR& s )

{

C[0] *= s;

C[1] *= s;

C[2] *= s;

return *this;

}

//self-multiply by a matrix

const MATRIX& operator *= ( const MATRIX& m )

{

//NOTE: don’t change the columns

//in the middle of the operation

MATRIX temp = (*this);

C[0] = temp * m.C[0];

C[1] = temp * m.C[1];

C[2] = temp * m.C[2];

return *this;

}

//add

const MATRIX operator + ( const MATRIX& m ) const

{

return MATRIX( C[0] + m.C[0], C[1] + m.C[1], C[2] + m.C[2] );

}

//subtract

const MATRIX operator - ( const MATRIX& m ) const

{

return MATRIX( C[0] - m.C[0], C[1] - m.C[1], C[2] - m.C[2] );

}

//post-multiply by a scalar

const MATRIX operator * ( const SCALAR& s ) const

{

return MATRIX( C[0]*s, C[1]*s, C[2]*s );

}

//pre-multiply by a scalar

friend inline const MATRIX operator * ( const SCALAR& s, const MATRIX& m )

{

return m * s;

}

//post-multiply by a vector

const VECTOR operator * ( const VECTOR& v ) const

{

return( C[0]*v.x + C[1]*v.y + C[2]*v.z );

}

//pre-multiply by a vector

inline friend const VECTOR operator * ( const VECTOR& v, const MATRIX& m )

{

return VECTOR( m.C[0].dot(v), m.C[1].dot(v), m.C[2].dot(v) );

}

//post-multiply by a matrix

const MATRIX operator * ( const MATRIX& m ) const

{

return MATRIX( (*this) * m.C[0], (*this) * m.C[1], (*this) * m.C[2] );

}

//transpose

MATRIX transpose() const

{

//turn columns on their side

return MATRIX( VECTOR( C[0].x, C[1].x, C[2].x ), //column 0

VECTOR( C[0].y, C[1].y, C[2].y ), //column 1

VECTOR( C[0].z, C[1].z, C[2].z ) //column 2

);

}

//scalar determinant

const SCALAR determinant() const

{

//Lang, "Linear Algebra", p. 143

return C[0].dot( C[1].cross(C[2]) );

}

//matrix inverse

const MATRIX inverse() const;

};

The BASIS type

There are two good ways and one bad way to store three-dimensional orientation. Storing it as a quaternion uses only four scalars and makes interpolating between orientations efficient. Storing orientation as an orthonormal basis (three mutually orthogonal unit vectors in the form of a 3x3 matrix) uses nine scalars and is less convenient for interpolating orientation; however, it is much more efficient for transforming vectors to and from a local coordinate frame. Because of collision detection this seems to be what the orientation is most frequently used for, so I prefer using an orthonormal basis (see Listing 3). The conversion from a basis to a quaternion is not terribly expensive and can be done if interpolation is necessary.

Why define a BASIS type when you can simply use a 3x3 matrix? There are operations on bases that you wouldn’t do on matrices, such as rotation, and there are matrix operations (such as addition) that don’t make sense on bases. The distinct type makes the code a little cleaner, as well as safeguarding against unwanted operations.

To transform a vector v in body space to world space, we multiply the x, y and z components of v by their corresponding axes and add all the resulting vectors together:

VECTOR v_world = v.x * X_body + v.y * Y_body + v.z * Z_body;

To transform a vector v in world space back to body space, we dot the vector with each body axis:

v_body.x = v.dot( X_body );

v_body.y = v.dot( Y_body );

v_body.z = v.dot( Z_body );

Listing 3.

// An orthonormal basis with respect to a parent

//

class BASIS

{

public:

MATRIX R;

public:

BASIS()

{}

BASIS(

const VECTOR& v0,

const VECTOR& v1,

const VECTOR& v2

)

: R( v0, v1, v2 )

{}

BASIS( const MATRIX& m )

: R( m )

{}

const VECTOR& operator [] ( long i ) const { return R.C[i]; }

const VECTOR& x() const { return R.C[0]; }

const VECTOR& y() const { return R.C[1]; }

const VECTOR& z() const { return R.C[2]; }

const MATRIX& basis() const { return R; }

void basis( const VECTOR& v0, const VECTOR& v1, const VECTOR& v2 )

{

this->R[0] = v0;

this->R[1] = v1;

this->R[2] = v2;

}

// Right-Handed Rotations

void rotateAboutX( const SCALAR& a )

{

if( 0 != a )//don’t rotate by 0

{

VECTOR b1 = this->Y()*cos(a) + this->Z()*sin(a);

VECTOR b2 = -this->Y()*sin(a) + this->Z()*cos(a);

//set basis

this->M[1] = b1;

this->M[2] = b2;

//x is unchanged

}

}

void rotateAboutY( const SCALAR& a )

{

if( 0 != a )//don’t rotate by 0

{

VECTOR b2 = this->Z()*cos(a) + this->X()*sin(a); //rotate z

VECTOR b0 = -this->Z()*sin(a) + this->X()*cos(a); //rotate x

//set basis

this->M[2] = b2;

this->M[0] = b0;

//y is unchanged

}

}

void rotateAboutZ( const SCALAR& a )

{

if( 0 != a )//don’t rotate by 0

{

//don’t over-write basis before calculation is done

VECTOR b0 = this->X()*cos(a) + this->Y()*sin(a); //rotate x

VECTOR b1 = -this->X()*sin(a) + this->Y()*cos(a); //rotate y

//set basis

this->M[0] = b0;

this->M[1] = b1;

//z is unchanged

}

}

//rotate the basis about the unit axis u by theta (radians)

void rotate( const SCALAR& theta, const VECTOR& u );

//rotate, length of da is theta, unit direction of da is u

void rotate( const VECTOR& da );

// Transformations

const VECTOR transformVectorToLocal( const VECTOR& v ) const

{

return VECTOR( R.C[0].dot(v), R.C[1].dot(v), R.C[2].dot(v) );

}

const POINT transformVectorToParent( const VECTOR& v ) const

{

return R.C[0] * v.x + R.C[1] * v.y + R.C[2] * v.z;

}

};

The COORDINATE_FRAME type

Just as the (x,y,z) values of a vector depend on the basis you are using, the (x,y,z) values of a point in 3D space depend on which coordinate frame you are using. For instance, the (x,y,z) position you are sitting at could be specified with respect to your chair, the corner of the room, the center of the Earth, or the center of the Sun. And just like vectors, your (x,y,z) coordinates will vary depending on the orientation of the coordinate frame.

In Listing 4 all that was done to create the COORDINATE_FRAME type was to derive from BASIS and add a position member (known as the origin of the frame). Deriving from BASIS will preserve the ability to transform vectors to and from the local frame and rotate the frame. Specifying an origin allows us to transform points to and from the local frame, as well as translate and position the coordinate frame.

It’s important to remember that the basis and the origin are specified with respect to a parent frame. For example, if a tire’s parent is the car, the tire’s basis and position must be in car coordinates.

Listing 4.

// A coordinate frame (basis and origin) with respect to a parent

//

class COORD_FRAME : public BASIS

{

public:

POINT O; //this coordinate frame’s origin, relative to its parent frame

public:

COORD_FRAME()

{}

COORD_FRAME(

const POINT& o,

const VECTOR& v0,

const VECTOR& v1,

const VECTOR& v2

)

: O ( o ),

BASIS ( v0, v1, v2 )

{}

COORD_FRAME(

const POINT& o,

const BASIS& b

)

: O ( o ),

BASIS ( b )

{}

const POINT& position() const { return O; }

void position( const POINT& p ) { O = p; }

const POINT transformPointToLocal( const POINT& p ) const

{

//translate to this frame’s origin, then project onto this basis

return transformVectorToLocal( p - O );

}

const POINT transformPointToParent( const POINT& p ) const

{

//transform the coordinate vector and translate by this origin

return transformVectorToParent( p ) + O;

}

//translate the origin by the given vector

void translate( const VECTOR& v )

{

O += v;

}

};

The RIGID_BODY_STATE type

The name says it all. The RIGID_BODY_STATE keeps track of the dynamic variables of the body. It is created by deriving from COORDINATE_FRAME and including the linear velocity, the angular velocity, the inertia tensor and its inverse (see Listing 5).

Listing 5

// Describe the dynamic state of a rigid
// body

class RIGID_BODY_STATE : public COORD_FRAME

{

public:

public:

};

The RIGID_BODY type

Finally, we create the structure that allows use to fully describe and manipulate a rigid body in 3D space. The object hierarchy of the RIGID_BODY class allows us to perform all the fundamental operations: positioning, translation, rotation, transformation, and linear and angular acceleration. In order to perform collision detection, you also need to store the previous state of the body. The current and previous states will allow you to interpolate between states to check for overlaps with the world or other bodies. This is why we separated the state information from the actual rigid body class.

In Listing 6, you’ll notice that RIGID_BODY is a class instead of a structure. Up to this point I’ve broken the object-oriented rule of data hiding because I’m lazy and I like to access members directly. The RIGID_BODY will be our interface with the rest of the world, so data hiding will do us good at this point.

Listing 6.

// Describe a rigid body

//

class RIGID_BODY : public RIGID_BODY_STATE

{

public:

//previous dynamic state

RIGID_BODY_STATE PrevState;

//mass and rotational inertia

SCALAR M; //mass, kg

VECTOR Ip; //principal moments of inertia, kg m m

public:

RIGID_BODY()

: M(1), Ip(1,1,1)

{}

RIGID_BODY( const SCALAR& m, const VECTOR& ip )

: M(m), Ip(ip)

{}

//physical properties

void mass( const SCALAR& m ) { M = m; }

SCALAR mass() const { return M; }

void inertiaMoments( const VECTOR& ip ) { Ip = ip; }

const VECTOR& inertiaMoments() const { return Ip; }

};

What have we gained?

So what have we gained by creating all these structures and operators? Here is an example of C++ code that will move a body under a force and a torque according to the Newton-Euler equations of rigid body motion:

void RIGID_BODY::move( const SCALAR dt )

{

VECTOR F, T;

this->Translate( this->V * dt );

this->Rotate( this->W * dt );

ComputeForceAndTorque( F, T );

this->V += (1/this->M)* F * dt;

this->W += this->I_inv * (T - W.cross(I*W)) * dt;

}

The equivalent C code is less elegant:

void RIGID_BODY_Move( RIGID_BODY* pBody, SCALAR dt )

{

VECTOR F, T, t, v;

//easier to hand-inline this one

pBody->State.Frame.O.x += pBody->State.V.x * dt;

pBody->State.Frame.O.y += pBody->State.V.y * dt;

pBody->State.Frame.O.z += pBody->State.V.z * dt;

vmul( t, pBody->State.W, dt );//macro

RotateBasis( pBody->State.Frame.Basis, &t );

ComputeForceAndTorque( &F, &T );

s = 1/ pBody->M;

pBody->State.V.x += s * F.x * dt;

pBody->State.V.y += s * F.y * dt;

pBody->State.V.z += s * F.z * dt;

matrix_mul( t, pBody->State.I, pBody->State.W );//t = I*W

vcross( v, pBody->State.W, t ); //v = Wx(I*W)

vsub( t, T, v ); //t = T - Wx(I*W)

matrix_mul( v, pBody->State.I_inv, t );//v = I_inv*(T - Wx(I*W))

vmul( t, v, dt ); //t = I_inv * (T - Wx(I*W)) * dt

pBody->State.W.x += t.x;

pBody->State.W.y += t.y;

pBody->State.W.z += t.z;

}

Using a good set of data structures, we’ve gained simplicity while preserving efficiency. With a little time spent up front on design, the entire physics and collision detection code base will be smaller, faster and easier to debug.

Miguel Gomez is a Senior Software Engineer at Looking Glass Studios, Redmond. Since receiving a BS in Physics from the University of Washington in 1995, he has programmed physics and collision detection for PGA Tour Golf '96" Hyperblade, Baseball 3D, 1998 Edition, Destruction Derby 64, and most recently Wild Waters, a 3-D kayaking simulation title for the Nintendo 64. And if it weren’t for the computer gaming industry, he’d be unemployed.

Bibliography

[1] Tai L. Chow, Classical Mechanics

[2] Harry F. Davis, Arthur David Snider, Introduction to Vector Analysis, 6^th Edition

[3] Brian W. Kernighan, Dennis M. Ritchie, The C Programming Language, 2^nd Edition

[4] Serge Lang, Linear Algebra, 3^rd Edition

[5] Paul Pedriana, High Performance Game Programming in C++, Conference Proceedings 1998 Computer Game Developer’s Conference

[6] Bjarne Stroustrup, The C++ Programming Language, 2^nd Edition