Let's write a c++ math libraryI (part 2)

Last time we made a class to encapsulate a 2D vector with floating point entries.  In this post we'll finish up with the 3 and 4 dimensional vector classes.  None of these will be particularly useful however, until we make a matrix class that can transform these vectors.

So let's get started.

vector3f.h

#ifndef _VECTOR3F_H_
#define _VECTOR3F_H_

class vector3f
{
private:
    float elements[3];
public:

    static const int SIZE;

    vector3f(float x = 0.0f, float y = 0.0f, float z = 0.0f);

    vector3f(const vector3f& other);

    vector3f normal() const;

    void normalize();

    float length() const;

    static float dot(const vector3f& vector1, const vector3f& vector2);

    static vector3f cross(const vector3f& vector1, const vector3f& vector2);

    vector3f& operator=(const vector3f& other);

    vector3f operator-() const;

    vector3f operator*(float scalar) const;

    vector3f operator/(float scalar) const;

    friend vector3f operator+(const vector3f& vector1, const vector3f& vector2);

    friend vector3f operator-(const vector3f& vector1, const vector3f& vector2);

    friend vector3f operator*(float scalar, const vector3f& vector);

    friend bool operator==(const vector3f& vector1, const vector3f& vector2);

    friend bool operator!=(const vector3f& vector1, const vector3f& vector2);

    float& operator[](int index);
};

#endif

You'll notice we have a function that the vector2f class does not:  cross
This is the cross product of two 3D vectors which produces a vector perpendicular to both input vectors in a direction determined by the right hand rule.  It is only defined for 3 dimensional vectors.

vector3f.cpp

#include "vector3f.h"
#include <cmath>

const int vector3f::SIZE = 3;

vector3f::vector3f(float x, float y, float z)
{
    elements[0] = x;
    elements[1] = y;
    elements[2] = z;
}

vector3f::vector3f(const vector3f& other)
{
    elements[0] = other.elements[0];
    elements[1] = other.elements[1];
    elements[2] = other.elements[2];
}

vector3f vector3f::normal() const
{
    float l = length();
    return vector3f(elements[0] / l, elements[1] / l, elements[2] / l);
}

void vector3f::normalize()
{
    float l = length();
    elements[0] /= l;
    elements[1] /= l;
    elements[2] /= l;
}

float vector3f::length() const
{
    return std::sqrt(elements[0] * elements[0] + elements[1] * elements[1] + elements[2] * elements[2]);
}

float vector3f::dot(const vector3f& vector1, const vector3f& vector2)
{
    return vector1.elements[0] * vector2.elements[0] + vector1.elements[1] * vector2.elements[1] +
        vector1.elements[2] * vector2.elements[2];
}

vector3f vector3f::cross(const vector3f& vector1, const vector3f& vector2)
{
    return vector3f(vector1.elements[1] * vector2.elements[2] - vector1.elements[2] * vector2.elements[0],
        vector1.elements[2] * vector2.elements[0] - vector1.elements[0] * vector2.elements[2],
        vector1.elements[0] * vector2.elements[1] - vector1.elements[1] * vector2.elements[0]);
}

vector3f& vector3f::operator=(const vector3f& other)
{
    if (this != &other)
    {
        elements[0] = other.elements[0];
        elements[1] = other.elements[1];
        elements[2] = other.elements[2];
    }
        return *this;
}

vector3f vector3f::operator-() const
{
    return vector3f(-elements[0], -elements[1], -elements[2]);
}

vector3f vector3f::operator*(float scalar) const
{
    return vector3f(scalar * elements[0], scalar * elements[1], scalar * elements[2]);
}

vector3f vector3f::operator/(float scalar) const
{
    return vector3f(elements[0] / scalar, elements[1] / scalar, elements[2] / scalar);
}

vector3f operator+(const vector3f& vector1, const vector3f& vector2)
{
    return vector3f(vector1.elements[0] + vector2.elements[0], vector1.elements[1] + vector2.elements[1],
        vector1.elements[2] * vector2.elements[2]);
}

vector3f operator-(const vector3f& vector1, const vector3f& vector2)
{
    return vector3f(vector1.elements[0] - vector2.elements[0], vector1.elements[1] - vector2.elements[1],
        vector1.elements[2] - vector2.elements[2]);
}

vector3f operator*(float scalar, const vector3f& vector)
{
    return vector3f(scalar * vector.elements[0], scalar * vector.elements[1],
        scalar * vector.elements[2]);
}

bool operator==(const vector3f& vector1, const vector3f& vector2)
{
    return ((vector1.elements[0] == vector2.elements[0]) && (vector1.elements[1] == vector2.elements[1]) &&
        vector1.elements[2] == vector2.elements[2]);
}

bool operator!=(const vector3f& vector1, const vector3f& vector2)
{
    return ((vector1.elements[0] != vector2.elements[0]) || (vector1.elements[1] != vector2.elements[1]) ||
        vector1.elements[2] != vector2.elements[2]);
}

float& vector3f::operator[](int index)
{
    return elements[index];
}

vector4f.h

#ifndef _VECTOR4F_H_
#define _VECTOR4F_H_

class KOGAPI vector4f
{
private:
    float elements[4];
 
public:

    static const int SIZE;

    vector4f(float x = 0.0f, float y = 0.0f, float z = 0.0f, float w = 0.0f);

    vector4f(const vector4f& other);

    vector4f normal() const;

    void normalize();

    float length() const;

    static float dot(const vector4f& vector1, const vector4f& vector2);

    vector4f& operator=(const vector4f& other);

    vector4f operator-() const;

    vector4f operator*(float scalar) const;

    vector4f operator/(float scalar) const;

    friend vector4f operator+(const vector4f& vector1, const vector4f& vector2);

    friend vector4f operator-(const vector4f& vector1, const vector4f& vector2);

    friend vector4f operator*(float scalar, const vector4f& vector);

    friend bool operator==(const vector4f& vector1, const vector4f& vector2);

    friend bool operator!=(const vector4f& vector1, const vector4f& vector2);

    float& operator[](int index);
};

#endif

vector4f.cpp

#include "vector4f.h"
#include <cmath>

vector4f::vector4f(float x, float y, float z, float w)
{
    elements[0] = x;
    elements[1] = y;
    elements[2] = z;
    elements[3] = w;
}

vector4f::vector4f(const vector4f& other)
{
    elements[0] = other.elements[0];
    elements[1] = other.elements[1];
    elements[2] = other.elements[2];
    elements[3] = other.elements[3];
}

vector4f vector4f::normal() const
{
    float l = length();
    return vector4f(elements[0] / l, elements[1] / l, elements[2] / l, elements[3] / l);
}

void vector4f::normalize()
{
    float l = length();
    elements[0] /= l;
    elements[1] /= l;
    elements[2] /= l;
    elements[3] /= l;
}

float vector4f::length() const
{
    return std::sqrt(elements[0] * elements[0] + elements[1] * elements[1] + elements[2] * elements[2] +
        elements[3] * elements[3]);
}

float vector4f::dot(const vector4f& vector1, const vector4f& vector2)
{
    return vector1.elements[0] * vector2.elements[0] + vector1.elements[1] * vector2.elements[1] +
        vector1.elements[2] * vector2.elements[2] + vector1.elements[3] * vector2.elements[3];
}

vector4f& vector4f::operator=(const vector4f& other)
{
    if (this != &other)
    {
        elements[0] = other.elements[0];
        elements[1] = other.elements[1];
        elements[2] = other.elements[2];
        elements[3] = other.elements[3];
    }
    return *this;
}

vector4f vector4f::operator-() const
{
    return vector4f(-elements[0], -elements[1], -elements[2], -elements[3]);
}

vector4f vector4f::operator*(float scalar) const
{
    return vector4f(scalar * elements[0], scalar * elements[1], scalar * elements[2],
        scalar * elements[3]);
}

vector4f vector4f::operator/(float scalar) const
{
    return vector4f(elements[0] / scalar, elements[1] / scalar, elements[2] / scalar,
        elements[3] / scalar);
}

vector4f operator+(const vector4f& vector1, const vector4f& vector2)
{
    return vector4f(vector1.elements[0] + vector2.elements[0], vector1.elements[1] + vector2.elements[1],
        vector1.elements[2] * vector2.elements[2], vector1.elements[3] + vector2.elements[3]);
}

vector4f operator-(const vector4f& vector1, const vector4f& vector2)
{
    return vector4f(vector1.elements[0] - vector2.elements[0], vector1.elements[1] - vector2.elements[1],
        vector1.elements[2] - vector2.elements[2], vector1.elements[3] - vector2.elements[3]);
}

vector4f operator*(float scalar, const vector4f& vector)
{
    return vector4f(scalar * vector.elements[0], scalar * vector.elements[1],
        scalar * vector.elements[2], scalar * vector.elements[3]);
}

bool operator==(const vector4f& vector1, const vector4f& vector2)
{
    return ((vector1.elements[0] == vector2.elements[0]) && (vector1.elements[1] == vector2.elements[1]) &&
        vector1.elements[2] == vector2.elements[2] && vector1.elements[3] == vector2.elements[3]);
}

bool operator!=(const vector4f& vector1, const vector4f& vector2)
{
    return ((vector1.elements[0] != vector2.elements[0]) || (vector1.elements[1] != vector2.elements[1]) ||
        (vector1.elements[2] != vector2.elements[2]) || (vector1.elements[3] != vector2.elements[3]));
}

float& vector4f::operator[](int index)
{
    return elements[index];
}

That's it for now.  Next post we'll set up the matrix and quaternion class.
Questions are always welcome and if you have an idea for a tutorial or series you'd like to see feel free to comment it!.

Let's write a c++ math library!

Wow, it's been a long time since I've updated this blog. So I've really been getting into C++ and OpenGL lately.
Why C++?
Well,

• It's been around forever
• It's easier to write cross platform applications
• Unlike C it's object oriented out of the box
• Pointers are fun
• I like it.  Whatever, I know someone who puts garlic on their peanut butter sandwiches, go bother him.

If you don't know C++ there's endless resources out there.  So I'd like to start by writing a math library for rendering graphics.  This means linear algebra.  Go beef up on your linear algebra if you don't know much about it.  Here's an MIT course on the subject.  Have fun.

Let's get started!  First we'll need a vector class.  A few actually so let's start with a 2D vector with floating point elements.

vector2f.h

#ifndef _VECTOR2F_H_
#define _VECTOR2F_H_

class vector2f
{
private:
    float elements[2];

public:
    vector2f(float x = 0.0f, float y = 0.0f);

    vector2f(const vector2f& other);
    
    float length() const;

    static float dot(const vector2f& vector1, const vector2f& vector2);

    vector2f operator-();
    
    vector2f operator*(float scalar);

    friend operator*(float scalar, const vector2f& vector);

    friend operator/(const vector2f& vector, float scalar);

    friend operator+(const vector2f& vector1, const vector2f& vector2);

    friend operator-(const vector2f& vector1, const vector2f& vector2);

    vector2f& operator=(const vector2f& other);

    float& operator[](int index);

    friend bool operator==(const vector2f& vector1, const vector2f& vector2);

    friend bool operator!=(const vector2f& vector1, const vector2f& vector2);
};

#endif

That's good for now but we'll probably add more to this class later.  Let's take a look at the source code.

vector2f.cpp

#include "vector2f.h"
#include <cmath>

vector2f::vector2f(float x, float y)
{
 elements[0] = x;
 elements[1] = y;
}

vector2f::vector2f(const vector2f& other)
{
    elements[0] = other.elements[0];
    elements[1] = other.elements[1];
}

float vector2f::length() const
{
    return std::sqrt(elements[0] * elements[0] + elements[1] * elements[1]);
}

float vector2f::dot(const vector2f& vector1, const vector2f& vector2)
{
    return vector1.elements[0] * vector2.elements[0] + vector1.elements[1] * vector2.elements[1];
}

vector2f vector2f::operator-()
{
    return vector2f(-elements[0], -elements[1]);
}

vector2f vector2f::operator*(float scalar)
{
    return vector2f(scalar * elements[0], scalar * elements[1]);
}

vector2f operator*(float scalar, const vector2f& vector)
{
    return vector2f(scalar * vector.elements[0], scalar * vector.elements[1]);
}

vector2f operator/(const vector2f& vector, float scalar)
{
    return vector2f(vector.elements[0] / scalar, vector.elements[1] / scalar);
}

vector2f operator+(const vector2f& vector1, const vector2f& vector2)
{
    return vector2f(vector1.elements[0] + vector2.elements[0], vector1.elements[1] + vector2.elements[1]);
}

vector2f operator-(const vector2f& vector1, const vector2f& vector2)
{
    return vector2f(vector1.elements[0] - vector2.elements[0], vector1.elements[1] - vector2.elements[1]);
}

vector2f& vector2f::operator=(const vector2f& other)
{
    if (this != &other)
    {
     elements[0] = other.elements[0];
     elements[1] = other.elements[1];
    }
    return *this;
}

float& vector2f::operator[](int index)
{
     //unsafe!  TODO:  add bounds checking
     return elements[index];
}

bool operator==(const vector2f& vector1, const vector2f& vector2)
{
    return (vector1.elements[0] == vector2.elements[0] &&
        vector1.elements[1] == vector2.elements[1]);
}

bool operator!=(const vector2f& vector1, const vector2f& vector2)
{
    return (vector1.elements[0] != vector2.elements[0] ||
        vector1.elements[1] != vector2.elements[1]);
}

Hey, alright!  That should be pretty good for now.  This is pretty basic stuff but I thought someone might benefit from seeing how it's done.  Later we'll add matrix classes and even a quaternion class for rotation.
That's it for now, though!

next:  part 2

The GJK Algorithm

This code sample uses the GJK Algorithm to determine if two convex regions in 3-space are intersecting.
There's a good tutorial on how this algorithm actually works here and another one here. This is my implementation in c#.
One can extend this to handle any convex shape at all as long as it has a function to determine the furthest point in the shape along a give direction, ie. if one were to walk along a line determined by a direction, starting "behind" the shape, determine the last point of the shape that you'll pass. This basically boils down to finding the shape's position that has the largest dot product with the direction.
While this can be tricky, GJK can handle any convex shape that implements this function making it very flexible. It's also faster than many other methods and uses minimal resources.
Now on to the code.
PhysicsExtensionMethods static class:

static class PhysicsExtensionMethods
{
    public static bool IsInSameDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) > 0;
    }

    public static bool IsInOppositeDirection(this Vector3 vector, Vector3 otherVector)
    {
        return Vector3.Dot(vector, otherVector) < 0;
    }
}

IConvexRegion interface:

public interface IConvexRegion
{
    /// <summary>
    /// Calculates the furthest point on the region 
    /// along a given direction.
    /// </summary>
    Vector3 GetFurthestPoint(Vector3 direction);
}

Simplex class:

/// <summary>
/// Represents a generalized Tetrehedron
/// </summary>
class Simplex
{
    List<Vector3> _vertices =
        new List<Vector3>();

    public int Count
    {
        get { return _vertices.Count; }
    }

    public Vector3 this[int i]
    {
        get { return _vertices[i]; }
    }

    public Simplex(params Vector3[] vertices)
    {
        for (int i = 0; i < vertices.Length; i++)
        {
            _vertices.Add(vertices[i]);
        }
    }
      
    public void Add(Vector3 vertex)
    {
        _vertices.Add(vertex);
    }

    public void Remove(Vector3 vertex)
    {
        _vertices.Remove(vertex);
    }
}

GJKAlgorithm static class:

public static class GJKAlgorithm
{
    public static bool Intersects(IConvexRegion regioneOne, IConvexRegion regionTwo)
    {
        //Get an initial point on the Minkowski difference.
        Vector3 s = Support(regioneOne, regionTwo, Vector3.One);
        
        //Create our initial simplex.
        Simplex simplex = new Simplex(s);

        //Choose an initial direction toward the origin.
        Vector3 d = -s;

        //Choose a maximim number of iterations to avoid an 
        //infinite loop during a non-convergent search.
        int maxIterations = 50;

        for (int i = 0; i < maxIterations; i++)
        {
            //Get our next simplex point toward the origin.
            Vector3 a = Support(regioneOne, regionTwo, d);

            //If we move toward the origin and didn't pass it 
            //then we never will and there's no intersection.
            if (a.IsInOppositeDirection(d))
            {
                return false;
            }
            //otherwise we add the new
            //point to the simplex and
            //process it.
            simplex.Add(a);
            //Here we either find a collision or we find the closest feature of
            //the simplex to the origin, make that the new simplex and update the direction
            //to move toward the origin from that feature.
            if (ProcessSimplex(ref simplex, ref d))
            {
                return true;
            }
        }
        //If we still couldn't find a simplex 
        //that contains the origin then we
        //"probably" have an intersection.
        return true;
    }

    /// <summary>
    ///Either finds a collision or the closest feature of the simplex to the origin, 
    ///and updates the simplex and direction.
    /// </summary>
    static bool ProcessSimplex(ref Simplex simplex, ref Vector3 direction)
    {
        if (simplex.Count == 2)
        {
            return ProcessLine(ref simplex, ref direction);
        }
        else if (simplex.Count == 3)
        {
            return ProcessTriangle(ref simplex, ref direction);
        }
        else
        {
            return ProcessTetrehedron(ref simplex, ref direction);
        }
    }

    /// <summary>
    /// Determines which Veronoi region of a line segment 
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessLine(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[1];
        Vector3 b = simplex[0];
        Vector3 ab = b - a;
        Vector3 aO = -a;

        if (ab.IsInSameDirection(aO))
        {
            float dot = Vector3.Dot(ab, aO);
            float angle = (float)Math.Acos(dot / (ab.Length() * aO.Length()));
            direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
        }
        else
        {
            simplex.Remove(b);
            direction = aO;
        }
        return false;
    }

    /// <summary>
    /// Determines which Veronoi region of a triangle 
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessTriangle(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[2];
        Vector3 b = simplex[1];
        Vector3 c = simplex[0];
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 abc = Vector3.Cross(ab, ac);
        Vector3 aO = -a;
        Vector3 acNormal = Vector3.Cross(abc, ac);
        Vector3 abNormal = Vector3.Cross(ab, abc);

        if (acNormal.IsInSameDirection(aO))
        {
            if (ac.IsInSameDirection(aO))
            {
                simplex.Remove(b);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
        }
        else
        {
            if (abNormal.IsInSameDirection(aO))
            {
                if (ab.IsInSameDirection(aO))
                {
                    simplex.Remove(c);
                    direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
                }
                else
                {
                    simplex.Remove(b);
                    simplex.Remove(c);
                    direction = aO;
                }
            }
            else
            {
                if (abc.IsInSameDirection(aO))
                {
                    direction = Vector3.Cross(Vector3.Cross(abc, aO), abc);
                }
                else
                {
                    direction = Vector3.Cross(Vector3.Cross(-abc, aO), -abc);
                }
            }
        }
        return false;
    }

    /// <summary>
    /// Determines which Veronoi region of a tetrahedron
    /// the origin is in, utilizing the preserved winding
    /// of the simplex to eliminate certain regions.
    /// </summary>
    static bool ProcessTetrehedron(ref Simplex simplex, ref Vector3 direction)
    {
        Vector3 a = simplex[3];
        Vector3 b = simplex[2];
        Vector3 c = simplex[1];
        Vector3 d = simplex[0];
        Vector3 ac = c - a;
        Vector3 ad = d - a;
        Vector3 ab = b - a;
        Vector3 bc = c - b;
        Vector3 bd = d - b;
            
        Vector3 acd = Vector3.Cross(ad, ac);
        Vector3 abd = Vector3.Cross(ab, ad);
        Vector3 abc = Vector3.Cross(ac, ab);
            
        Vector3 aO = -a;

        if (abc.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abc, ac).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else if (Vector3.Cross(ab, abc).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else
            {
                simplex.Remove(d);
                direction = abc;
            }
        }
        else if (acd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(acd, ad).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else if (Vector3.Cross(ac, acd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ac, aO), ac);
            }
            else
            {
                simplex.Remove(b);
                direction = acd;
            }
        }
        else if (abd.IsInSameDirection(aO))
        {
            if (Vector3.Cross(abd, ab).IsInSameDirection(aO))
            {
                simplex.Remove(c);
                simplex.Remove(d);
                direction = Vector3.Cross(Vector3.Cross(ab, aO), ab);
            }
            else if (Vector3.Cross(ad, abd).IsInSameDirection(aO))
            {
                simplex.Remove(b);
                simplex.Remove(c);
                direction = Vector3.Cross(Vector3.Cross(ad, aO), ad);
            }
            else
            {
                simplex.Remove(c);
                direction = abd;
            }
        }
        else
        {
            return true;
        }

        return false;
    }

    /// <summary>
    /// Calculates the furthest point on the Minkowski 
    /// difference along a given direction.
    /// </summary>
    static Vector3 Support(
        IConvexRegion regionOne, 
        IConvexRegion regionTwo,
        Vector3 direction)
    {
        return regionOne.GetFurthestPoint(direction) -
            regionTwo.GetFurthestPoint(-direction);
    }
}

Sphere class:

public class Sphere : IConvexRegion
{
    public Vector3 Center;
    public float Radius;

    public Sphere(Vector3 center, float radius)
    {
        Center = center;
        Radius = radius;
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        if (direction != Vector3.Zero)
        {
            direction.Normalize();
        }
        return Center + Radius * direction;
    }
}

Box class:

public class Box : IConvexRegion
{
    public Vector3 Center;
    Vector3 _halfDimensions = Vector3.One;
    Quaternion _orientation = Quaternion.Identity;

    public Vector3 Dimensions
    {
        get { return 2f * _halfDimensions; }
    }

    public Box(Vector3 center)
        : this(center, 1f, 1f, 1f) { }

    public Box(Vector3 center,
        float width,
        float height,
        float depth)
        : this(center, width, height, depth, Matrix.Identity) { }

    public Box(Vector3 center,
        float width,
        float height,
        float depth,
        Matrix rotationMatrix)
    {
        Center = center;
        _halfDimensions = new Vector3(
            width / 2f,
            height / 2f,
            depth / 2f);
        _orientation = Quaternion.CreateFromRotationMatrix(rotationMatrix);
    }

    public Vector3 GetFurthestPoint(Vector3 direction)
    {
        Vector3 halfHeight = _halfDimensions.Y * Vector3.Up;
        Vector3 halfWidth = _halfDimensions.X * Vector3.Right;
        Vector3 halfDepth = _halfDimensions.Z * Vector3.Backward;

        Vector3[] vertices = new Vector3[8];
        vertices[0] = halfWidth + halfHeight + halfDepth;
        vertices[1] = -halfWidth + halfHeight + halfDepth;
        vertices[2] = halfWidth - halfHeight + halfDepth;
        vertices[3] = halfWidth + halfHeight - halfDepth;
        vertices[4] = -halfWidth - halfHeight + halfDepth;
        vertices[5] = halfWidth - halfHeight - halfDepth;
        vertices[6] = -halfWidth + halfHeight - halfDepth;
        vertices[7] = -halfWidth - halfHeight - halfDepth;

        Matrix rotationTransform = Matrix.CreateFromQuaternion(_orientation);
        Matrix translation = Matrix.CreateTranslation(Center);
        Matrix world = rotationTransform *
            translation;

        Vector3 furthestPoint = Vector3.Transform(vertices[0], world);
        float maxDot = Vector3.Dot(furthestPoint, direction);
        for (int i = 1; i < 8; i++)
        {
            Vector3 vertex = Vector3.Transform(vertices[i], world);
            float dot = Vector3.Dot(vertex, direction);
            if (dot > maxDot)
            {
                maxDot = dot;
                furthestPoint = vertex;
            }               
        }
        return furthestPoint;
    }

    public Matrix CalculateWorld()
    {
        return Matrix.CreateScale(Dimensions) *
            Matrix.CreateFromQuaternion(_orientation) *
            Matrix.CreateTranslation(Center);
    }
}

I hope this is helpful to someone :) Enjoy!