diff --git a/math/ray.cpp b/math/ray.cpp
index b50cd0e594c..33f3232553f 100644
--- a/math/ray.cpp
+++ b/math/ray.cpp
@@ -45,6 +45,11 @@ void Ray::rotate(const Quaternion &rot) {
 	_direction.normalize();
 }
 
+void Ray::rotateDirection(const Quaternion &rot) {
+	rot.transform(_direction);
+	_direction.normalize();
+}
+
 void Ray::translate(const Vector3d &v) {
 	_origin += v;
 }
@@ -78,4 +83,38 @@ bool Ray::intersectAABB(const AABB &aabb) const {
 	return true;
 }
 
+// Algorithm adapted from https://www.lighthouse3d.com/tutorials/maths/ray-triangle-intersection/
+bool Ray::intersectTriangle(const Vector3d &v0, const Vector3d &v1,
+		const Vector3d &v2, Vector3d &vout, float &fout) const {
+	const Vector3d e1 = v1 - v0;
+	const Vector3d e2 = v2 - v0;
+	const Vector3d h = Vector3d::crossProduct(_direction, e2);
+
+	float a = e1.dotProduct(h);
+	if (fabs(a) < 1e-6f)
+		return false;
+
+	float f = 1.0f / a;
+	const Vector3d s = _origin - v0;
+	float u = f * s.dotProduct(h);
+	if (u < 0.0f || u > 1.0f)
+		return false;
+
+	const Vector3d q = Vector3d::crossProduct(s, e1);
+	float v = f * _direction.dotProduct(q);
+
+	if (v < 0.0f || u + v > 1.0f)
+		return false;
+
+	float t = f * e2.dotProduct(q);
+
+	if (t < 1e-6f)
+		return false;
+
+	fout = t;
+	vout = _origin + t * _direction;
+
+	return true;
+}
+
 }
diff --git a/math/ray.h b/math/ray.h
index b09f8b4af18..fb6a8293db6 100644
--- a/math/ray.h
+++ b/math/ray.h
@@ -48,10 +48,15 @@ public:
 	void transform(const Matrix4 &matrix);
 
 	/**
-	 * Rotate the ray using a quaternion
+	 * Rotate the ray using a quaternion - rotates both origin and direction
 	 */
 	void rotate(const Quaternion &rot);
 
+	/**
+	 * Rotate the ray direction only using a quaternion - origin stays fixed
+	 */
+	void rotateDirection(const Quaternion &rot);
+
 	/**
 	 * Translate the ray by a vector
 	 */
@@ -62,6 +67,17 @@ public:
 	 */
 	bool intersectAABB(const AABB &aabb) const;
 
+	/**
+	 * Test and return the intersection of the ray with a triangle defned by 3 verticies
+	 *
+	 * @param v0 first triangle vertex
+	 * @param v1 second triangle vertex
+	 * @param v2 third triangle vertex
+	 * @param loc If return is true, set to the intersection point
+	 * @param dist If return is true, set to distance along the ray (relative to direction)
+	 */
+	bool intersectTriangle(const Vector3d &v0, const Vector3d &v1, const Vector3d &v2, Vector3d &loc, float &dist) const;
+
 private:
 	Vector3d _origin;
 	Vector3d _direction;
diff --git a/test/math/ray.h b/test/math/ray.h
new file mode 100644
index 00000000000..5345583d0f3
--- /dev/null
+++ b/test/math/ray.h
@@ -0,0 +1,62 @@
+#include <cxxtest/TestSuite.h>
+
+#include "math/ray.h"
+
+class RayTestSuite : public CxxTest::TestSuite {
+public:
+	// Test Constructors
+	void test_Ray() {
+		Math::Ray r;
+
+		TS_ASSERT(r.getOrigin() == Math::Vector3d());
+		TS_ASSERT(r.getDirection() == Math::Vector3d());
+
+		Math::Vector3d o(3, 2, 1);
+		Math::Vector3d d(0, 1, 2);
+
+		Math::Ray r2(o, d);
+
+		TS_ASSERT(r2.getOrigin() == o);
+		TS_ASSERT(r2.getDirection() == d);
+	}
+
+	void test_translate() {
+		Math::Vector3d o(3, 2, 1);
+		Math::Vector3d d(0, 1, 2);
+		Math::Ray r(o, d);
+
+		r.translate(Math::Vector3d(0.5, 0.2, 0.1));
+		TS_ASSERT(r.getDirection() == d);
+		Math::Vector3d o2 = r.getOrigin();
+
+		TS_ASSERT_DELTA(o2.x(), 3.5, 0.0001);
+		TS_ASSERT_DELTA(o2.y(), 2.2, 0.0001);
+		TS_ASSERT_DELTA(o2.z(), 1.1, 0.0001);
+	}
+
+	// TODO: Add tests for transform, rotate, rotateDirection, intersectAABB
+	void test_intersectTriangle() {
+		// A triangle that covers around the origin on the y plane.
+		const Math::Vector3d v1(0, 0, -20);
+		const Math::Vector3d v2(0, -10, 20);
+		const Math::Vector3d v3(0, 10, 20);
+
+		// A ray that points along the x axis, should hit the triangle at the origin
+		Math::Ray r(Math::Vector3d(-9.5, 0, 0.7), Math::Vector3d(1, 0, 0));
+
+		Math::Vector3d loc(7, 8, 9); // add values to ensure it's changed
+		float dist = 99.0f;
+		bool result = r.intersectTriangle(v1, v2, v3, loc, dist);
+		// Should hit at the origin
+		TS_ASSERT(result);
+		TS_ASSERT_DELTA(dist, 9.5f, 0.0001);
+		TS_ASSERT_DELTA(loc.x(), 0.0f, 0.0001);
+		TS_ASSERT_DELTA(loc.y(), 0.0f, 0.0001);
+		TS_ASSERT_DELTA(loc.z(), 0.7f, 0.0001);
+
+		// A ray that points along the x axis in the opposite direction, should never hit the triangle
+		Math::Ray r2(Math::Vector3d(-1, 0, 0), Math::Vector3d(-1, 0, 0));
+		result = r2.intersectTriangle(v1, v2, v3, loc, dist);
+		TS_ASSERT(!result);
+	}
+};
diff --git a/test/math/vector3d.h b/test/math/vector3d.h
new file mode 100644
index 00000000000..8534bdc9a24
--- /dev/null
+++ b/test/math/vector3d.h
@@ -0,0 +1,144 @@
+#include <cxxtest/TestSuite.h>
+
+#include "math/vector3d.h"
+
+class Vector3dTestSuite : public CxxTest::TestSuite {
+public:
+	// Test Constructors
+	void test_Vector3d() {
+		Math::Vector3d v;
+
+		TS_ASSERT(v.x() == 0.0f);
+		TS_ASSERT(v.y() == 0.0f);
+		TS_ASSERT(v.z() == 0.0f);
+
+		Math::Vector3d v2(3, 2.2, 1);
+
+		TS_ASSERT(v2.x() == 3.0f);
+		TS_ASSERT(v2.y() == 2.2f);
+		TS_ASSERT(v2.z() == 1.0f);
+
+		Math::Vector3d v3(v2);
+
+		TS_ASSERT(v3.x() == 3.0f);
+		TS_ASSERT(v3.y() == 2.2f);
+		TS_ASSERT(v3.z() == 1.0f);
+	}
+
+	void test_set() {
+		Math::Vector3d v(2, 4, 6);
+		v.set(1, 2, 3);
+
+		TS_ASSERT(v.x() == 1.0f);
+		TS_ASSERT(v.y() == 2.0f);
+		TS_ASSERT(v.z() == 3.0f);
+	}
+
+	void test_crossProduct() {
+		Math::Vector3d v1(1, 0, 0);
+		Math::Vector3d v2(0, 1, 0);
+
+		Math::Vector3d c12 = Math::Vector3d::crossProduct(v1, v2);
+		Math::Vector3d c21 = Math::Vector3d::crossProduct(v2, v1);
+
+		TS_ASSERT(c12 == Math::Vector3d(0, 0, 1));
+		TS_ASSERT(c21 == Math::Vector3d(0, 0, -1));
+	}
+
+	void test_length() {
+		Math::Vector3d v(1, 0, 1);
+		TS_ASSERT_DELTA(v.length(), sqrt(2), 0.0001);
+
+		Math::Vector3d v2(2, 2, 2);
+		TS_ASSERT_DELTA(v2.length(), sqrt(12), 0.0001);
+
+		// check static version too
+		TS_ASSERT_DELTA(Math::Vector3d::length(v2), v2.length(), 0.0001);
+	}
+
+	void test_interpolate() {
+		Math::Vector3d v1(1, 0, 2);
+		Math::Vector3d v2(0, 5, 3);
+
+		Math::Vector3d int1 = Math::Vector3d::interpolate(v1, v2, 0.1);
+		TS_ASSERT_DELTA(int1.x(), 0.9, 0.0001);
+		TS_ASSERT_DELTA(int1.y(), 0.5, 0.0001);
+		TS_ASSERT_DELTA(int1.z(), 2.1, 0.0001);
+	}
+
+	/* all the below tests are for functions in Math::Vector, but add tests
+	 * here as we can't directly instantiate that */
+
+	void test_setValue() {
+		Math::Vector3d v(3, 7, 9);
+
+		v.setValue(0, 1);
+		TS_ASSERT(v.x() == 1.0f);
+		TS_ASSERT(v.y() == 7.0f);
+		TS_ASSERT(v.z() == 9.0f);
+
+		v.setValue(1, 3);
+		v.setValue(2, 2);
+		TS_ASSERT(v.x() == 1.0f);
+		TS_ASSERT(v.y() == 3.0f);
+		TS_ASSERT(v.z() == 2.0f);
+	}
+
+	void test_getValue() {
+		Math::Vector3d v(5, 6, 7);
+
+		TS_ASSERT(v.getValue(0) == 5.0f);
+		TS_ASSERT(v.getValue(1) == 6.0f);
+		TS_ASSERT(v.getValue(2) == 7.0f);
+	}
+
+	void test_dotProduct() {
+		Math::Vector3d v1(1, 2, 3);
+		Math::Vector3d v2(6, 5, 4);
+
+		float result = v2.dotProduct(v1);
+
+		TS_ASSERT_DELTA(result, 28.0f, 0.0001f);
+	}
+
+	void test_dotProductOrthogonal() {
+		Math::Vector3d v1(6, 0, 0);
+		Math::Vector3d v2(0, 9, 0);
+
+		float result = v2.dotProduct(v1);
+
+		TS_ASSERT_EQUALS(result, 0.0f);
+	}
+
+	void test_normalizeTrivial() {
+		Math::Vector3d v(0, 1, 0);
+
+		Math::Vector3d vn = v.getNormalized();
+
+		v.normalize();
+
+		TS_ASSERT_EQUALS(vn, v);
+		TS_ASSERT(v.x() == 0);
+		TS_ASSERT(v.y() == 1);
+		TS_ASSERT(v.z() == 0);
+	}
+
+	void test_normalize() {
+		Math::Vector3d v(2, 4, 6);
+
+		Math::Vector3d vn = v.getNormalized();
+		v.normalize();
+
+		TS_ASSERT_EQUALS(vn, v);
+		TS_ASSERT_DELTA(v.x(), 2 / sqrt(56), 0.0001);
+		TS_ASSERT_DELTA(v.y(), 4 / sqrt(56), 0.0001);
+		TS_ASSERT_DELTA(v.z(), 6 / sqrt(56), 0.0001);
+	}
+
+	void test_magnitude() {
+		Math::Vector3d v(3, 2.5, 1);
+
+		TS_ASSERT_DELTA(v.getMagnitude(), sqrt(16.25), 0.0001);
+	}
+
+};