Move to implementation file

include/J3ML/Geometry.h

@@ -14,6 +14,10 @@ namespace Geometry {
     };
 
     class Rectangle;
+    class AABB;
+    class OBB;
+    class Capsule;
+    class Frustum;
     class OBB2D;
     class Line2D;
     class Ray2D;

include/J3ML/Geometry/AABB.h

@@ -1,6 +1,10 @@
 #pragma once
 
 #include <J3ML/LinearAlgebra/Vector3.h>
+
+#include <J3ML/LinearAlgebra.h>
+#include <J3ML/Geometry.h>
+
 #include <J3ML/Geometry/Plane.h>
 #include <J3ML/Geometry/Sphere.h>
 #include <J3ML/Geometry/OBB.h>
@@ -33,21 +37,75 @@ namespace Geometry
 
     class AABB
     {
-        static AABB FromCenterAndSize(const Vector3 FromSize);
-        float MinX();
-        // Returns the smallest sphere that contains this AABB.
-        // This function computes the minimal volume sphere that contains all the points inside this AABB
-        Sphere MinimalEnclosingSphere() const;
-        // Returns the largest sphere that can fit inside this AABB
+    public:
+        Vector3 minPoint;
+        Vector3 maxPoint;
+
+        static int NumFaces() { return 6; }
+        static int NumEdges() { return 12;}
+        static int NumVertices() { return 8;}
+
+        static AABB FromCenterAndSize(const Vector3& center, const Vector3& size)
+        {
+            Vector3 halfSize =  size * 0.5f;
+            return {center - halfSize, center + halfSize};
+        }
+        float MinX() const { return minPoint.x; }
+        float MinY() const { return minPoint.y; }
+        float MinZ() const { return minPoint.z; }
+
+        float MaxX() const { return maxPoint.x; }
+        float MaxY() const { return maxPoint.y; }
+        float MaxZ() const { return maxPoint.z; }
+
+
+        /// Returns the smallest sphere that contains this AABB.
+        /// This function computes the minimal volume sphere that contains all the points inside this AABB
+        Sphere MinimalEnclosingSphere() const
+        {
+            return Sphere(Centroid(), Size().Length()*0.5f);
+        }
+
+        Vector3 HalfSize() const {
+            return this->Size()/2.f;
+        }
+
+// Returns the largest sphere that can fit inside this AABB
         // This function computes the largest sphere that can fit inside this AABB.
-        Sphere MaximalContainedSphere() const;
-        Vector3 GetCentroid() const;
+        Sphere MaximalContainedSphere() const
+        {
+            Vector3 halfSize = HalfSize();
+            return Sphere(Centroid(), std::min(halfSize.x, std::min(halfSize.y, halfSize.z)));
+        }
+        bool IsFinite() const
+        {
+            return minPoint.IsFinite() && maxPoint.IsFinite();
+        }
+        Vector3 Centroid() const
+        {
+            return (minPoint+maxPoint) * 0.5f;
+        }
+        Vector3 Size() const
+        {
+            return this->maxPoint - this->minPoint;
+        }
         // Quickly returns an arbitrary point inside this AABB
         Vector3 AnyPointFast() const;
 
-        Vector3 PointInside(float x, float y, float z) const;
+        Vector3 PointInside(float x, float y, float z) const
+        {
+            Vector3 d = maxPoint - minPoint;
+            return minPoint + d.Mul({x, y, z});
+        }
         // Returns an edge of this AABB
-        LineSegment Edge(int edgeIndex) const;
+        LineSegment Edge(int edgeIndex) const
+        {
+            switch(edgeIndex)
+            {
+                default:
+                case 0: return LineSegment(minPoint, {minPoint.x, minPoint.y, maxPoint.z});
+            }
+        }
         Vector3 CornerPoint(int cornerIndex);
         Vector3 ExtremePoint(const Vector3& direction) const;
         Vector3 ExtremePoint(const Vector3& direction, float projectionDistance);
@@ -57,7 +115,6 @@ namespace Geometry
         Vector3 FaceNormal(int faceIndex) const;
         Plane FacePlane(int faceIndex);
         static AABB MinimalEnclosingAABB(const Vector3* pointArray, int numPoints);
-        Vector3 GetSize();
         Vector3 GetVolume();
         float GetVolumeCubed();
         float GetSurfaceArea();

include/J3ML/Geometry/Capsule.h

@@ -13,10 +13,10 @@ namespace Geometry
         // Specifies the radius of this capsule
         float r;
 
-        Capsule() {}
+        Capsule();
         Capsule(const LineSegment& endPoints, float radius);
         Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
-        bool IsDegenerate()const;
+        bool IsDegenerate() const;
         float Height() const;
         float Diameter() const;
         Vector3 Bottom() const;

include/J3ML/Geometry/Frustum.h

@@ -36,19 +36,5 @@ namespace Geometry
         static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
     };
 
-    Frustum Frustum::CreateFrustumFromCamera(const CoordinateFrame &cam, float aspect, float fovY, float zNear, float zFar) {
-        Frustum frustum;
-        const float halfVSide = zFar * tanf(fovY * 0.5f);
-        const float halfHSide = halfVSide * aspect;
 
-        const Vector3 frontMultFar = cam.Front * zFar;
-
-        frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
-        frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
-        frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
-        frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
-        frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
-        frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
-        return frustum;
-    }
 }

include/J3ML/Geometry/LineSegment.h

@@ -7,6 +7,9 @@ namespace Geometry
     using LinearAlgebra::Vector3;
     class LineSegment
     {
+    public:
+        LineSegment();
+        LineSegment(const Vector3& a, const Vector3& b);
         Vector3 A;
         Vector3 B;
     };

include/J3ML/Geometry/OBB.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <J3ML/Geometry.h>
 #include <J3ML/Geometry/AABB.h>
 #include <J3ML/Geometry/LineSegment.h>
 #include <J3ML/Geometry/Polyhedron.h>

include/J3ML/Geometry/QuadTree.h

@@ -6,10 +6,21 @@
 #include "AABB2D.h"
 
 namespace Geometry {
+
+
     using LinearAlgebra::Vector2;
 
     template<typename T>
     class QuadTree {
+        /// A fixed split rule for all QuadTrees: A QuadTree leaf node is only ever split if the leaf contains at least this many objects.
+        /// Leaves containing fewer than this many objects are always kept as leaves until the object count is exceeded.
+        constexpr static const int minQuadTreeNodeObjectCount = 16;
+
+        /// A fixed split limit rule for all QuadTrees: If the QuadTree node side length is smaller than this, the node will
+        /// never be split again into smaller subnodes. This provides a hard limit safety net for infinite/extra long recursion
+        /// in case multiple identical overlapping objects are placed into the tree.
+        constexpr static const float minQuadTreeQuadrantSize = 0.05f;
+
     public:
         struct Node {
             Node *parent;

include/J3ML/Geometry/Ray.h

@@ -8,6 +8,7 @@
 
 namespace Geometry
 {
+    using LinearAlgebra::Vector3;
     class Ray
     {
         Vector3 Origin;

include/J3ML/Geometry/Sphere.h

@@ -1,9 +1,15 @@
 #pragma once
 
+#include "J3ML/Geometry.h"
+
 namespace Geometry
 {
     class Sphere
     {
+    public:
+        Sphere(const Vector3& pos, float radius)
+        {
 
+        }
     };
 }

include/J3ML/LinearAlgebra/CoordinateFrame.h

@@ -8,6 +8,7 @@ namespace LinearAlgebra
     /// The CFrame is fundamentally 4 vectors (position, forward, right, up vector)
     class CoordinateFrame
     {
+    public:
         Vector3 Position;
         Vector3 Front;
         Vector3 Right;

include/J3ML/LinearAlgebra/Matrix3x3.h

@@ -44,6 +44,7 @@ namespace LinearAlgebra {
         Vector3 GetRow(int index) const;
         Vector3 GetColumn(int index) const;
 
+        float &At(int row, int col);
         float At(int x, int y) const;
 
         void SetRotatePart(const Vector3& a, float angle);
@@ -132,7 +133,11 @@ namespace LinearAlgebra {
         Vector2 Transform(const Vector2& rhs) const;
         Vector3 Transform(const Vector3& rhs) const;
 
-        Vector2 operator * (const Vector2& rhs) const;
+        Vector3 operator[](int row) const
+        {
+            return Vector3{elems[row][0], elems[row][1], elems[row][2]};
+        }
+
         Vector3 operator * (const Vector3& rhs) const;
         Matrix3x3 operator * (const Matrix3x3& rhs) const;
 

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -12,7 +12,7 @@ namespace LinearAlgebra {
      * The elements of this matrix are
      *  m_00, m_01, m_02, m_03
      *  m_10, m_11, m_12, m_13
-     *  m_20, m_21, m_22, am_23,
+     *  m_20, m_21, m_22, m_23,
      *  m_30, m_31, m_32, m_33
      *
      *  The element m_yx is the value on the row y and column x.
@@ -60,6 +60,9 @@ namespace LinearAlgebra {
 
         explicit Matrix4x4(const Quaternion& orientation);
 
+        /// Constructs this float4x4 from the given quaternion and translation.
+        /// Logically, the translation occurs after the rotation has been performed.
+        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);
 
         Vector3 GetTranslatePart() const;
         Matrix3x3 GetRotatePart() const
@@ -73,6 +76,8 @@ namespace LinearAlgebra {
         void SetTranslatePart(float translateX, float translateY, float translateZ);
         void SetTranslatePart(const Vector3& offset);
         void SetRotatePart(const Quaternion& q);
+        void Set3x3Part(const Matrix3x3& r);
+
 
 
         void SetRow(int row, const Vector3& rowVector, float m_r3);
@@ -80,10 +85,9 @@ namespace LinearAlgebra {
         void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
 
 
-        Matrix4x4(const Quaternion& orientation, const Vector3& translation);
-
         Vector4 GetRow(int index) const;
         Vector4 GetColumn(int index) const;
+        float &At(int row, int col);
         float At(int x, int y) const;
 
         /// Tests if this matrix does not contain any NaNs or infs.
@@ -107,11 +111,23 @@ namespace LinearAlgebra {
 
         Matrix4x4 Transpose() const;
 
+        Vector2 Transform(float tx, float ty) const;
         Vector2 Transform(const Vector2& rhs) const;
+
+
+        Vector3 Transform(float tx, float ty, float tz) const;
         Vector3 Transform(const Vector3& rhs) const;
+
+        Vector4 Transform(float tx, float ty, float tz, float tw) const;
         Vector4 Transform(const Vector4& rhs) const;
 
 
+        Matrix4x4 Translate(const Vector3& rhs) const;
+        static Matrix4x4 FromTranslation(const Vector3& rhs);
+
+
+
+
         static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
@@ -128,61 +144,34 @@ namespace LinearAlgebra {
         Vector4 GetRow() const;
         Vector4 GetColumn() const;
 
-        Vector4 operator[](int row)
-        {
-            return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
-        }
+        Vector4 operator[](int row);
 
         Matrix4x4 operator-() const;
         Matrix4x4 operator +(const Matrix4x4& rhs) const;
         Matrix4x4 operator - (const Matrix4x4& rhs) const;
 
-        Matrix4x4 operator *(float scalar) const
-        {
-
-        }
-
-        Vector2 operator * (const Vector2& rhs) const;
-        Vector3 operator * (const Vector3& rhs) const;
-        Vector4 operator * (const Vector4& rhs) const;
-
-        Matrix4x4 operator * (const Matrix3x3 &rhs) const
-        {
-
-        }
-
-        Matrix4x4 operator +() const { return *this; }
-
-        Matrix4x4 operator * (const Matrix4x4& rhs) const
-        {
+        Matrix4x4 operator *(float scalar) const;
+        Matrix4x4 operator /(float scalar) const;
 
 
-            float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
-            float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
-            float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
-            float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
 
-            float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
-            float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
-            float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
-            float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
+        Vector2 operator * (const Vector2& rhs) const { return this->Transform(rhs);}
+        Vector3 operator * (const Vector3& rhs) const { return this->Transform(rhs);}
+        Vector4 operator * (const Vector4& rhs) const { return this->Transform(rhs);}
 
-            float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
-            float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
-            float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
-            float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
+        Matrix4x4 operator * (const Matrix3x3 &rhs) const;
 
-            float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
-            float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
-            float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
-            float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
-            return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
-        }
+        Matrix4x4 operator +() const;
 
+        Matrix4x4 operator * (const Matrix4x4& rhs) const;
+
+        Matrix4x4 &operator = (const Matrix3x3& rhs);
+        Matrix4x4 &operator = (const Quaternion& rhs);
+        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;
 
     protected:
         float elems[4][4];
 
-        void SetMatrixRotatePart(Matrix4x4 &m, const Quaternion &q);
+
     };
 }

include/J3ML/LinearAlgebra/Quaternion.h

@@ -8,51 +8,64 @@
 
 namespace LinearAlgebra
 {
-    class Quaternion : public Vector4
-    {
+    class Quaternion : public Vector4 {
     public:
         Quaternion();
-        Quaternion(const Quaternion& rhs) = default;
-        explicit Quaternion(const Matrix3x3& rotationMtrx);
-        explicit Quaternion(const Matrix4x4& rotationMtrx);
+
+        Quaternion(const Quaternion &rhs) = default;
+
+        explicit Quaternion(const Matrix3x3 &rotationMtrx);
+
+        explicit Quaternion(const Matrix4x4 &rotationMtrx);
+
         // @note The input data is not normalized after construction, this has to be done manually.
         Quaternion(float X, float Y, float Z, float W);
 
         // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
         // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
-        Quaternion(const Vector3& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
-        Quaternion(const Vector4& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
+        Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween) {
+            SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+        }
+
+        Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
+            SetFromAxisAngle(rotationAxis, rotationAngleBetween);
+        }
         //void Inverse();
 
         explicit Quaternion(Vector4 vector4);
 
         void SetFromAxisAngle(const Vector3 &vector3, float between);
+
         void SetFromAxisAngle(const Vector4 &vector4, float between);
 
         Quaternion Inverse() const;
+
         Quaternion Conjugate() const;
 
         //void Normalize();
         Vector3 GetWorldX() const;
+
         Vector3 GetWorldY() const;
+
         Vector3 GetWorldZ() const;
 
-        Vector3 GetAxis() const
-        {
+        Vector3 GetAxis() const {
             float rcpSinAngle = 1 - (std::sqrt(1 - w * w));
 
             return Vector3(x, y, z) * rcpSinAngle;
         }
 
-        float GetAngle() const
-        {
+        float GetAngle() const {
             return std::acos(w) * 2.f;
         }
 
 
-
         Matrix3x3 ToMatrix3x3() const;
 
+        Matrix4x4 ToMatrix4x4() const;
+
+        Matrix4x4 ToMatrix4x4(const Vector3 &translation) const;
+
         Vector3 Transform(const Vector3& vec) const;
         Vector3 Transform(float X, float Y, float Z) const;
         // Note: We only transform the x,y,z components of 4D vectors, w is left untouched
@@ -89,7 +102,7 @@ namespace LinearAlgebra
         Quaternion operator - () const;
         float Dot(const Quaternion &quaternion) const;
 
-        float Angle() const;
+        float Angle() const { return std::acos(w) * 2.f;}
 
         float AngleBetween(const Quaternion& target) const;
 

include/J3ML/LinearAlgebra/Vector2.h

@@ -1,8 +1,12 @@
 #pragma once
+#include <J3ML/J3ML.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cstddef>
 
 namespace LinearAlgebra {
+    using namespace J3ML;
+
+
     /// A 2D (x, y) ordered pair.
     class Vector2 {
     public:
@@ -65,6 +69,11 @@ namespace LinearAlgebra {
         float Magnitude() const;
         static float Magnitude(const Vector2& of);
 
+
+
+
+
+
         bool IsFinite() const;
         static bool IsFinite(const Vector2& v);
 

include/J3ML/LinearAlgebra/Vector3.h

@@ -38,7 +38,7 @@ public:
     }
 
 
-    //Returns the DirectionVector for a given angle.
+    /// Returns the DirectionVector for a given angle.
     static Vector3 Direction(const Vector3 &rhs) ;
 
 
@@ -78,6 +78,11 @@ public:
     bool operator == (const Vector3& rhs) const;
     bool operator != (const Vector3& rhs) const;
 
+    bool IsFinite() const
+    {
+        return std::isfinite(x) && std::isfinite(y) && std::isfinite(z);
+    }
+
     Vector3 Min(const Vector3& min) const;
     static Vector3 Min(const Vector3& lhs, const Vector3& rhs);
 
@@ -87,7 +92,7 @@ public:
     Vector3 Clamp(const Vector3& min, const Vector3& max) const;
     static Vector3 Clamp(const Vector3& min, const Vector3& input, const Vector3& max);
 
-    // Returns the magnitude between the two vectors.
+    /// Returns the magnitude between the two vectors.
     float Distance(const Vector3& to) const;
     static float Distance(const Vector3& from, const Vector3& to);
 
@@ -97,33 +102,33 @@ public:
     float LengthSquared() const;
     static float LengthSquared(const Vector3& of);
 
-    // Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
+    /// Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
     float Magnitude() const;
     static float Magnitude(const Vector3& of);
 
-    // Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
-    // For normalized vectors, dot returns 1 if they point in exactly the same direction,
-    // -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
+    /// Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
+    /// For normalized vectors, dot returns 1 if they point in exactly the same direction,
+    /// -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
     float Dot(const Vector3& rhs) const;
     static float Dot(const Vector3& lhs, const Vector3& rhs);
 
-    // Projects one vector onto another and returns the result. (IDK)
+    /// Projects one vector onto another and returns the result. (IDK)
     Vector3 Project(const Vector3& rhs) const;
     static Vector3 Project(const Vector3& lhs, const Vector3& rhs);
 
-    // The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
-    // The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
+    /// The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
+    /// The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
     Vector3 Cross(const Vector3& rhs) const;
     static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
 
-    // Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
+    /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
     Vector3 Normalize() const;
     static Vector3 Normalize(const Vector3& targ);
 
-    // Linearly interpolates between two points.
-    // Interpolates between the points and b by the interpolant t.
-    // The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
-    // This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
+    /// Linearly interpolates between two points.
+    /// Interpolates between the points and b by the interpolant t.
+    /// The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
+    /// This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
     Vector3 Lerp(const Vector3& goal, float alpha) const;
     static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);
 
@@ -136,24 +141,38 @@ public:
     Vector3 Add(const Vector3& rhs) const;
     static Vector3 Add(const Vector3& lhs, const Vector3& rhs);
 
-    // Subtracts two vectors
+    /// Subtracts two vectors
     Vector3 operator-(const Vector3& rhs) const;
     Vector3 Sub(const Vector3& rhs) const;
     static Vector3 Sub(const Vector3& lhs, const Vector3& rhs);
 
-    // Multiplies this vector by a scalar value
+    /// Multiplies this vector by a scalar value
     Vector3 operator*(float rhs) const;
     Vector3 Mul(float scalar) const;
     static Vector3 Mul(const Vector3& lhs, float rhs);
 
-    // Divides this vector by a scalar
+    /// Multiplies this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    ///	 but this function is provided here for syntactical convenience.
+    Vector3 Mul(const Vector3& rhs) const
+    {
+
+    }
+
+    /// Divides this vector by a scalar
     Vector3 operator/(float rhs) const;
     Vector3 Div(float scalar) const;
     static Vector3 Div(const Vector3& lhs, float rhs);
 
-    // Unary + operator
+
+    /// Divides this vector by a vector, element-wise
+    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
+    /// but this function is provided here for syntactical convenience
+    Vector2 Div(const Vector2& v) const;
+
+    /// Unary + operator
     Vector3 operator+() const; // TODO: Implement
-    // Unary - operator (Negation)
+    /// Unary - operator (Negation)
     Vector3 operator-() const;
 public:
      float x = 0;

src/J3ML/Geometry/Capsule.cpp

@@ -3,4 +3,5 @@
 namespace Geometry
 {
 
+    Capsule::Capsule() : l() {}
 }

src/J3ML/Geometry/Frustum.cpp

@@ -1 +1,20 @@
 #include <J3ML/Geometry/Frustum.h>
+
+namespace Geometry
+{
+    Frustum Frustum::CreateFrustumFromCamera(const CoordinateFrame &cam, float aspect, float fovY, float zNear, float zFar) {
+        Frustum frustum;
+        const float halfVSide = zFar * tanf(fovY * 0.5f);
+        const float halfHSide = halfVSide * aspect;
+
+        const Vector3 frontMultFar = cam.Front * zFar;
+
+        frustum.NearFace  = Plane{cam.Position + cam.Front * zNear, cam.Front};
+        frustum.FarFace   = Plane{cam.Position + frontMultFar, -cam.Front};
+        frustum.RightFace = Plane{cam.Position, Vector3::Cross(frontMultFar - cam.Right * halfHSide, cam.Up)};
+        frustum.LeftFace  = Plane{cam.Position, Vector3::Cross(cam.Up, frontMultFar+cam.Right*halfHSide)};
+        frustum.TopFace   = Plane{cam.Position, Vector3::Cross(cam.Right, frontMultFar - cam.Up * halfVSide)};
+        frustum.BottomFace   = Plane{cam.Position, Vector3::Cross(frontMultFar + cam.Up * halfVSide, cam.Right)};
+        return frustum;
+    }
+}

src/J3ML/Geometry/LineSegment.cpp

@@ -2,4 +2,10 @@
 
 namespace Geometry {
 
+    LineSegment::LineSegment(const Vector3 &a, const Vector3 &b) : A(a), B(b)
+    {
+
+    }
+
+    LineSegment::LineSegment() {}
 }

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -296,5 +296,9 @@ namespace LinearAlgebra {
         };
     }
 
+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
 }
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -90,11 +90,8 @@ namespace LinearAlgebra {
         elems[2][3] = offset.z;
     }
 
-    void Matrix4x4::SetRotatePart(const Quaternion &q) {
-        SetMatrixRotatePart(*this, q);
-    }
 
-    void Matrix4x4::SetMatrixRotatePart(Matrix4x4 &m, const Quaternion& q)
+    void Matrix4x4::SetRotatePart(const Quaternion& q)
     {
         // See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
         const float x = q.x;
@@ -106,6 +103,15 @@ namespace LinearAlgebra {
         elems[2][0] =     2*(x*z - y*w); elems[2][1] =     2*(y*z + x*w); elems[2][2] = 1 - 2*(x*x + y*y);
     }
 
+    void Matrix4x4::Set3x3Part(const Matrix3x3& r)
+    {
+        At(0, 0) = r[0][0]; At(0, 1) = r[0][1]; At(0, 2) = r[0][2];
+        At(1, 0) = r[1][0]; At(1, 1) = r[1][1]; At(1, 2) = r[1][2];
+        At(2, 0) = r[2][0]; At(2, 1) = r[2][1]; At(2, 2) = r[2][2];
+    }
+
+
+
     void Matrix4x4::SetRow(int row, const Vector3 &rowVector, float m_r3) {
         SetRow(row, rowVector.x, rowVector.y, rowVector.z, m_r3);
     }
@@ -171,5 +177,136 @@ namespace LinearAlgebra {
         return elems[x][y];
     }
 
+    Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &rhs) const {
 
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0) + At(0, 3) * rhs.At(3, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1) + At(0, 3) * rhs.At(3, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2) + At(0, 3) * rhs.At(3, 2);
+        float r03 = At(0, 0) * rhs.At(0, 3) + At(0, 1) * rhs.At(1, 3) + At(0, 2) * rhs.At(2, 3) + At(0, 3) * rhs.At(3, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0) + At(1, 3) * rhs.At(3, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1) + At(1, 3) * rhs.At(3, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2) + At(1, 3) * rhs.At(3, 2);
+        float r13 = At(1, 0) * rhs.At(0, 3) + At(1, 1) * rhs.At(1, 3) + At(1, 2) * rhs.At(2, 3) + At(1, 3) * rhs.At(3, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0) + At(2, 3) * rhs.At(3, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1) + At(2, 3) * rhs.At(3, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2) + At(2, 3) * rhs.At(3, 2);
+        float r23 = At(2, 0) * rhs.At(0, 3) + At(2, 1) * rhs.At(1, 3) + At(2, 2) * rhs.At(2, 3) + At(2, 3) * rhs.At(3, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0) + At(3, 3) * rhs.At(3, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1) + At(3, 3) * rhs.At(3, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2) + At(3, 3) * rhs.At(3, 2);
+        float r33 = At(3, 0) * rhs.At(0, 3) + At(3, 1) * rhs.At(1, 3) + At(3, 2) * rhs.At(2, 3) + At(3, 3) * rhs.At(3, 3);
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Vector4 Matrix4x4::operator[](int row) {
+        return Vector4{elems[row][0], elems[row][1], elems[row][2], elems[row][3]};
+    }
+
+    Matrix4x4 Matrix4x4::operator*(const Matrix3x3 &rhs) const {
+        float r00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
+        float r01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
+        float r02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);
+        float r03 = At(0, 3);
+
+        float r10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
+        float r11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
+        float r12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2, 2);
+        float r13 = At(1, 3);
+
+        float r20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2, 2) * rhs.At(2, 0);
+        float r21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2, 2) * rhs.At(2, 1);
+        float r22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2, 2) * rhs.At(2, 2);
+        float r23 = At(2, 3);
+
+        float r30 = At(3, 0) * rhs.At(0, 0) + At(3, 1) * rhs.At(1, 0) + At(3, 2) * rhs.At(2, 0);
+        float r31 = At(3, 0) * rhs.At(0, 1) + At(3, 1) * rhs.At(1, 1) + At(3, 2) * rhs.At(2, 1);
+        float r32 = At(3, 0) * rhs.At(0, 2) + At(3, 1) * rhs.At(1, 2) + At(3, 2) * rhs.At(2, 2);
+        float r33 = At(3, 3);
+
+        return {r00,r01,r02,r03, r10, r11, r12, r13, r20,r21,r22,r23, r30,r31,r32,r33};
+    }
+
+    Matrix4x4 Matrix4x4::operator+() const { return *this; }
+
+    Matrix4x4 Matrix4x4::FromTranslation(const Vector3 &rhs) {
+        return Matrix4x4(1.f, 0, 0, rhs.x,
+                         0, 1.f, 0, rhs.y,
+                         0, 0, 1.f, rhs.z,
+                         0, 0, 0,     1.f);
+    }
+
+    Matrix4x4 Matrix4x4::Translate(const Vector3 &rhs) const {
+        return *this * FromTranslation(rhs);
+    }
+
+    Vector3 Matrix4x4::Transform(const Vector3 &rhs) const {
+        return Transform(rhs.x, rhs.y, rhs.z);
+    }
+
+    Vector3 Matrix4x4::Transform(float tx, float ty, float tz) const {
+        return Vector3(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) * tz + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) * tz + At(1, 3),
+                       At(2, 0) * tx + At(2, 1) * ty + At(2, 2) * tz + At(2, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(float tx, float ty) const {
+        return Vector2(At(0, 0) * tx + At(0, 1) * ty + At(0, 2) + At(0, 3),
+                       At(1, 0) * tx + At(1, 1) * ty + At(1, 2) + At(1, 3));
+    }
+
+    Vector2 Matrix4x4::Transform(const Vector2 &rhs) const {
+        return Transform(rhs.x, rhs.y);
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Matrix3x3 &rhs) {
+        Set3x3Part(rhs);
+        SetTranslatePart(0,0,0);
+        SetRow(3, 0, 0, 0, 1);
+        return *this;
+    }
+
+    Matrix4x4 &Matrix4x4::operator=(const Quaternion &rhs) {
+        *this = rhs.ToMatrix4x4();
+        return *this;
+    }
+
+    float &Matrix4x4::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Matrix4x4 Matrix4x4::Inverse() const {
+        // Compute the inverse directly using Cramer's rule
+        // Warning: This method is numerically very unstable!
+        float d = Determinant();
+
+        d = 1.f / d;
+
+        float a11 = At(0, 0);float a12 = At(0, 1);float a13 = At(0, 2);float a14 = At(0, 3);
+        float a21 = At(1, 0);float a22 = At(1, 1);float a23 = At(1, 2);float a24 = At(1, 3);
+        float a31 = At(2, 0);float a32 = At(2, 1);float a33 = At(2, 2);float a34 = At(2, 3);
+        float a41 = At(3, 0);float a42 = At(3, 1);float a43 = At(3, 2);float a44 = At(3, 3);
+
+        Matrix4x4 i = {
+                d * (a22*a33*a44 + a23*a34*a42 + a24*a32*a43 - a22*a34*a43 - a23*a32*a44 - a24*a33*a42),
+                d * (a12*a34*a43 + a13*a32*a44 + a14*a33*a42 - a12*a33*a44 - a13*a34*a42 - a14*a32*a43),
+                d * (a12*a23*a44 + a13*a24*a42 + a14*a22*a43 - a12*a24*a43 - a13*a22*a44 - a14*a23*a42),
+                d * (a12*a24*a33 + a13*a22*a34 + a14*a23*a32 - a12*a23*a34 - a13*a24*a32 - a14*a22*a33),
+                d * (a21*a34*a43 + a23*a31*a44 + a24*a33*a41 - a21*a33*a44 - a23*a34*a41 - a24*a31*a43),
+                d * (a11*a33*a44 + a13*a34*a41 + a14*a31*a43 - a11*a34*a43 - a13*a31*a44 - a14*a33*a41),
+                d * (a11*a24*a43 + a13*a21*a44 + a14*a23*a41 - a11*a23*a44 - a13*a24*a41 - a14*a21*a43),
+                d * (a11*a23*a34 + a13*a24*a31 + a14*a21*a33 - a11*a24*a33 - a13*a21*a34 - a14*a23*a31),
+                d * (a21*a32*a44 + a22*a34*a41 + a24*a31*a42 - a21*a34*a42 - a22*a31*a44 - a24*a32*a41),
+                d * (a11*a34*a42 + a12*a31*a44 + a14*a32*a41 - a11*a32*a44 - a12*a34*a41 - a14*a31*a42),
+                d * (a11*a22*a44 + a12*a24*a41 + a14*a21*a42 - a11*a24*a42 - a12*a21*a44 - a14*a22*a41),
+                d * (a11*a24*a32 + a12*a21*a34 + a14*a22*a31 - a11*a22*a34 - a12*a24*a31 - a14*a21*a32),
+                d * (a21*a33*a42 + a22*a31*a43 + a23*a32*a41 - a21*a32*a43 - a22*a33*a41 - a23*a31*a42),
+                d * (a11*a32*a43 + a12*a33*a41 + a13*a31*a42 - a11*a33*a42 - a12*a31*a43 - a13*a32*a41),
+                d * (a11*a23*a42 + a12*a21*a43 + a13*a22*a41 - a11*a22*a43 - a12*a23*a41 - a13*a21*a42),
+                d * (a11*a22*a33 + a12*a23*a31 + a13*a21*a32 - a11*a23*a32 - a12*a21*a33 - a13*a22*a31)
+        };
+        return i;
+    }
 }

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -1,6 +1,7 @@
 #include <J3ML/LinearAlgebra/Vector3.h>
 #include <J3ML/LinearAlgebra/Vector4.h>
 #include <J3ML/LinearAlgebra/Matrix3x3.h>
+#include <J3ML/LinearAlgebra/Matrix4x4.h>
 #include <J3ML/LinearAlgebra/Quaternion.h>
 
 namespace LinearAlgebra {
@@ -163,4 +164,12 @@ namespace LinearAlgebra {
                 x + rhs.x, y + rhs.y, z + rhs.z,w + rhs.w
         };
     }
+
+    Matrix4x4 Quaternion::ToMatrix4x4() const {
+        return Matrix4x4(*this);
+    }
+
+    Matrix4x4 Quaternion::ToMatrix4x4(const Vector3 &translation) const {
+        return {*this, translation};
+    }
 }

src/J3ML/LinearAlgebra/Transform2D.cpp

@@ -8,7 +8,7 @@ namespace LinearAlgebra {
 
 
     Vector2 Transform2D::Transform(const Vector2 &input) const {
-        return transformation * input;
+        return transformation.Transform(input);
     }
 
     Transform2D::Transform2D(const Matrix3x3 &transform) : transformation(transform) { }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -22,7 +22,6 @@ namespace LinearAlgebra {
         if (index == 1) return y;
         return 0;
     }
-
     bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
     {
         return this->Distance(rhs) <= margin;
@@ -247,5 +246,11 @@ namespace LinearAlgebra {
         return {this->x*v.x, this->y*v.y};
     }
 
+    bool Vector2::IsFinite() const {
+        return std::isfinite(x) && std::isfinite(y);
+    }
 
+    Vector2 Vector2::Div(const Vector2 &v) const {
+        return {this->x/v.x, this->y/v.y};
+    }
 }

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -9,6 +9,8 @@
 
 namespace LinearAlgebra {
 
+    const Vector4 Vector4::Zero = {0,0,0,0};
+    const Vector4 Vector4::NaN = {NAN, NAN, NAN, NAN};
 
     Vector4::Vector4(): x(0), y(0), z(0), w(0)
     {}