Implement Vector4 operator=

include/J3ML/Geometry/AABB.h

@@ -37,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);
@@ -61,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/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/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/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);
@@ -73,10 +74,7 @@ namespace LinearAlgebra {
 
         static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);
 
-        static Matrix3x3 FromQuat(const Quaternion& orientation)
-        {
-            return Matrix3x3(orientation);
-        }
+        static Matrix3x3 FromQuat(const Quaternion& orientation);
 
         Quaternion ToQuat() const;
 
@@ -86,14 +84,8 @@ namespace LinearAlgebra {
         // Transforming a vector v using this matrix computes the vector
         // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
         // is applied to the vector first, followed by rotation, and finally translation
-        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale)
-        {
-            return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
-        }
-        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale)
-        {
-            return rotate * Matrix3x3::Scale(scale);
-        }
+        static Matrix3x3 FromRS(const Quaternion& rotate, const Vector3& scale);
+        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Vector3& scale);
 
 
         /// Creates a new transformation matrix that scales by the given factors.
@@ -132,6 +124,10 @@ namespace LinearAlgebra {
         Vector2 Transform(const Vector2& rhs) const;
         Vector3 Transform(const Vector3& 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.
@@ -20,6 +20,7 @@ namespace LinearAlgebra {
      */
     class Matrix4x4 {
     public:
+        // TODO: Implement assertions to ensure matrix bounds are not violated!
         enum { Rows = 4 };
         enum { Cols = 4 };
 
@@ -43,9 +44,9 @@ namespace LinearAlgebra {
         /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
         /// E.g. The element _10 denotes the scalar at second (index 1) row, first (index 0) column.
         Matrix4x4(float m00, float m01, float m02, float m03,
-            float m10, float m11, float m12, float m13,
-            float m20, float m21, float m22, float m23,
-            float m30, float m31, float m32, float m33);
+                  float m10, float m11, float m12, float m13,
+                  float m20, float m21, float m22, float m23,
+                  float m30, float m31, float m32, float m33);
         /// Constructs the matrix by explicitly specifying the four column vectors.
         /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
             direction of the local X axis.
@@ -57,37 +58,92 @@ namespace LinearAlgebra {
             position of the local space pivot. */
         Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);
 
-
         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);
 
+        /// Creates a LookAt matrix from a look-at direction vector.
+        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
+            @param localForward Specifies the forward direction in the local space of the object. This is the direction
+                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
+                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
+                pick one convention for all your objects, and be consistent.
+                This input parameter must be a normalized vector.
+            @param targetDirection Specifies the desired world space direction the object should look at. This function
+                will compute a rotation matrix which will rotate the localForward vector to orient towards this targetDirection
+                vector. This input parameter must be a normalized vector.
+            @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
+                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
+                or your modeling software is using, and it is best to pick one convention for all your objects, and be
+                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
+                vector localForward, i.e. localForward.Dot(localUp) == 0.
+            @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
+                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
+                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
+                the up direction straight, this function orients the localUp direction of the model to point towards the
+                specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
+                collinear, but they do not need to be perpendicular either.
+            @return A matrix that maps the given local space forward direction vector to point towards the given target
+                direction, and the given local up direction towards the given target world up direction. The returned
+                matrix M is orthonormal with a determinant of +1. For the matrix M it holds that
+                M * localForward = targetDirection, and M * localUp lies in the plane spanned by the vectors targetDirection
+                and worldUp.
+            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
+                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
+            @see RotateFromTo(). */
+        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);
+
+        /// Returns the translation part.
+        /** The translation part is stored in the fourth column of this matrix.
+            This is equivalent to decomposing this matrix in the form M = T * M', i.e. this translation is applied last,
+            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
+            then this gives the world space position of the object.
+            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
         Vector3 GetTranslatePart() const;
-        Matrix3x3 GetRotatePart() const
-        {
-            return Matrix3x3 {
-                At(0, 0), At(0, 1), At(0, 2),
-                At(1, 0), At(1, 1), At(1, 2),
-                At(2, 0), At(2, 1), At(2, 2)
-            };
-        }
+        /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
+        Matrix3x3 GetRotatePart() const;
         void SetTranslatePart(float translateX, float translateY, float translateZ);
         void SetTranslatePart(const Vector3& offset);
         void SetRotatePart(const Quaternion& q);
-        void SetRotatePart(const Matrix3x3& r);
-
+        void Set3x3Part(const Matrix3x3& r);
 
         void SetRow(int row, const Vector3& rowVector, float m_r3);
         void SetRow(int row, const Vector4& rowVector);
         void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);
 
-
         Vector4 GetRow(int index) const;
         Vector4 GetColumn(int index) const;
+        Vector3 GetRow3(int index) const;
+        Vector3 GetColumn3(int index) const;
+
+        float &At(int row, int col);
         float At(int x, int y) const;
 
+        template <typename T>
+        void Swap(T &a, T &b)
+        {
+            T temp = std::move(a);
+            a = std::move(b);
+            b = std::move(temp);
+        }
+
+
+        void SwapColumns(int col1, int col2);
+
+        /// Swaps two rows.
+        void SwapRows(int row1, int row2);
+        /// Swapsthe xyz-parts of two rows element-by-element
+        void SwapRows3(int row1, int row2);
+
+        void ScaleRow(int row, float scalar);
+        void ScaleRow3(int row, float scalar);
+        void ScaleColumn(int col, float scalar);
+        void ScaleColumn3(int col, float scalar);
+        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
+        void Pivot();
+
         /// Tests if this matrix does not contain any NaNs or infs.
         /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
         bool IsFinite() const;
@@ -97,23 +153,52 @@ namespace LinearAlgebra {
         bool IsInvertible(float epsilon = 1e-3f) const;
 
         Vector4 Diagonal() const;
-        Vector4 WorldX() const;
-        Vector4 WorldY() const;
-        Vector4 WorldZ() const;
+        Vector3 Diagonal3() const;
+        /// Returns the local +X axis in world space.
+        /// This is the same as transforming the vector (1,0,0) by this matrix.
+        Vector3 WorldX() const;
+        /// Returns the local +Y axis in world space.
+        /// This is the same as transforming the vector (0,1,0) by this matrix.
+        Vector3 WorldY() const;
+        /// Returns the local +Z axis in world space.
+        /// This is the same as transforming the vector (0,0,1) by this matrix.
+        Vector3 WorldZ() const;
 
+        /// Accesses this structure as a float array.
+        /// @return A pointer to the upper-left element. The data is contiguous in memory.
+        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
+        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
+        float *ptr() { return &elems[0][0]; }
+        const float *ptr() const { return &elems[0][0]; }
+
+        float Determinant3x3() const;
         /// Computes the determinant of this matrix.
         // If the determinant is nonzero, this matrix is invertible.
         float Determinant() const;
 
+        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)
+
+        float Minor(int i, int j) const;
+
         Matrix4x4 Inverse() const;
 
         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);
@@ -124,12 +209,6 @@ namespace LinearAlgebra {
         static Matrix4x4 OpenGLPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 OpenGLPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
 
-        Vector3 GetTranslationComponent() const;
-        Matrix3x3 GetRotationComponent() const;
-
-        Vector4 GetRow() const;
-        Vector4 GetColumn() const;
-
         Vector4 operator[](int row);
 
         Matrix4x4 operator-() const;
@@ -139,9 +218,11 @@ namespace LinearAlgebra {
         Matrix4x4 operator *(float scalar) const;
         Matrix4x4 operator /(float scalar) const;
 
-        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);}
+
+
+        Vector2 operator * (const Vector2& rhs) const;
+        Vector3 operator * (const Vector3& rhs) const;
+        Vector4 operator * (const Vector4& rhs) const;
 
         Matrix4x4 operator * (const Matrix3x3 &rhs) const;
 
@@ -149,23 +230,13 @@ namespace LinearAlgebra {
 
         Matrix4x4 operator * (const Matrix4x4& rhs) const;
 
-        Matrix4x4 &operator = (const Matrix3x3& rhs)
-        {
-            SetRotatePart(rhs);
-            SetTranslatePart(0,0,0);
-            SetRow(3, 0, 0, 0, 1);
-            return *this;
-        }
-        Matrix4x4 &operator = (const Quaternion& rhs)
-        {
-            *this = rhs.ToMatrix4x4();
-            return *this;
-        }
+        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/Vector2.h

@@ -7,6 +7,8 @@ namespace LinearAlgebra {
     using namespace J3ML;
 
 
+
+
     /// A 2D (x, y) ordered pair.
     class Vector2 {
     public:
@@ -121,6 +123,10 @@ namespace LinearAlgebra {
         /// 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.
+        Vector2 operator *(const Vector2& rhs) const
+        {
+
+        }
         Vector2 Mul(const Vector2& v) const;
 
         /// Divides this vector by a scalar.
@@ -147,4 +153,9 @@ namespace LinearAlgebra {
         float y = 0;
 
     };
+
+    static Vector2 operator*(float lhs, const Vector2 &rhs)
+    {
+        return rhs * lhs;
+    }
 }

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;

include/J3ML/LinearAlgebra/Vector4.h

@@ -81,6 +81,8 @@ namespace LinearAlgebra {
 
         Vector4 operator+() const; // Unary + Operator
         Vector4 operator-() const; // Unary - Operator (Negation)
+
+
     public:
 #if MUTABLE
         float x;

src/J3ML/Geometry/Capsule.cpp

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

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,33 @@ namespace LinearAlgebra {
         };
     }
 
+    float &Matrix3x3::At(int row, int col) {
+        return elems[row][col];
+    }
+
+    Vector3 Matrix3x3::WorldZ() const {
+        return GetColumn(2);
+    }
+
+    Vector3 Matrix3x3::WorldY() const {
+        return GetColumn(1);
+    }
+
+    Vector3 Matrix3x3::WorldX() const {
+        return GetColumn(0);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Quaternion &rotate, const Vector3 &scale) {
+        return Matrix3x3(rotate) * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromRS(const Matrix3x3 &rotate, const Vector3 &scale) {
+        return rotate * Matrix3x3::Scale(scale);
+    }
+
+    Matrix3x3 Matrix3x3::FromQuat(const Quaternion &orientation) {
+        return Matrix3x3(orientation);
+    }
+
 }
 

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);
     }
@@ -224,4 +230,288 @@ namespace LinearAlgebra {
     }
 
     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;
+    }
+
+    float Matrix4x4::Minor(int i, int j) const {
+        int r0 = SKIPNUM(0, i);
+        int r1 = SKIPNUM(1, i);
+        int r2 = SKIPNUM(2, i);
+        int c0 = SKIPNUM(0, j);
+        int c1 = SKIPNUM(1, j);
+        int c2 = SKIPNUM(2, j);
+
+        float a = At(r0, c0);
+        float b = At(r0, c1);
+        float c = At(r0, c2);
+        float d = At(r1, c0);
+        float e = At(r1, c1);
+        float f = At(r1, c2);
+        float g = At(r2, c0);
+        float h = At(r2, c1);
+        float k = At(r2, c2);
+
+        return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g;
+    }
+
+    float Matrix4x4::Determinant() const {
+        return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3);
+    }
+
+    float Matrix4x4::Determinant3x3() const {
+
+        const float a = elems[0][0];
+        const float b = elems[0][1];
+        const float c = elems[0][2];
+        const float d = elems[1][0];
+        const float e = elems[1][1];
+        const float f = elems[1][2];
+        const float g = elems[2][0];
+        const float h = elems[2][1];
+        const float i = elems[2][2];
+
+        return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
+    }
+
+    Matrix3x3 Matrix4x4::GetRotatePart() const {
+        return Matrix3x3 {
+                At(0, 0), At(0, 1), At(0, 2),
+                At(1, 0), At(1, 1), At(1, 2),
+                At(2, 0), At(2, 1), At(2, 2)
+        };
+    }
+
+    Matrix4x4 Matrix4x4::Transpose() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    Vector4 Matrix4x4::Diagonal() const {
+        return Vector4{At(0, 0), At(1,1), At(2,2), At(3,3)};
+    }
+
+    Vector3 Matrix4x4::Diagonal3() const {
+        return Vector3 { At(0, 0), At(1,1), At(2,2) };
+    }
+
+    Vector3 Matrix4x4::WorldX() const {
+        return GetColumn3(0);
+    }
+
+    Vector3 Matrix4x4::WorldY() const {
+        return GetColumn3(1);
+    }
+
+    Vector3 Matrix4x4::WorldZ() const {
+        return GetColumn3(2);
+    }
+
+    bool Matrix4x4::IsFinite() const {
+        for(int iy = 0; iy < Rows; ++iy)
+            for(int ix = 0; ix < Cols; ++ix)
+                if (!std::isfinite(elems[iy][ix]))
+                    return false;
+        return true;
+    }
+
+    Vector3 Matrix4x4::GetColumn3(int index) const {
+        return Vector3{At(0, index), At(1, index), At(2, index)};
+    }
+
+    Vector2 Matrix4x4::operator*(const Vector2 &rhs) const { return this->Transform(rhs);}
+
+    Vector3 Matrix4x4::operator*(const Vector3 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::operator*(const Vector4 &rhs) const { return this->Transform(rhs);}
+
+    Vector4 Matrix4x4::Transform(float tx, float ty, float tz, float tw) const {
+        return Transform({tx, ty, tz, tw});
+    }
+
+    Vector4 Matrix4x4::Transform(const Vector4 &rhs) const {
+        return Vector4(At(0, 0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z + At(0, 3) * rhs.w,
+                       At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z + At(1, 3) * rhs.w,
+                       At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2, 2) * rhs.z + At(2, 3) * rhs.w,
+                       At(3, 0) * rhs.x + At(3, 1) * rhs.y + At(3, 2) * rhs.z + At(3, 3) * rhs.w);
+    }
+
+    Vector3 Matrix4x4::GetTranslatePart() const {
+        return GetColumn3(3);
+    }
+
+    Matrix4x4
+    Matrix4x4::LookAt(const Vector3 &localFwd, const Vector3 &targetDir, const Vector3 &localUp, const Vector3 &worldUp) {
+        Matrix4x4 m;
+        m.Set3x3Part(Matrix3x3::LookAt(localFwd, targetDir, localUp, worldUp));
+        m.SetTranslatePart(0,0,0);
+        m.SetRow(3, 0,0,0,1);
+        return m;
+    }
+
+    Vector4 Matrix4x4::GetRow(int index) const {
+        return { At(index, 0), At(index, 1), At(index, 2), At(index, 3)};
+    }
+
+    Vector4 Matrix4x4::GetColumn(int index) const {
+        return { At(0, index), At(1, index), At(2, index), At(3, index)};
+    }
+
+    Vector3 Matrix4x4::GetRow3(int index) const {
+        return Vector3{ At(index, 0), At(index, 1), At(index, 2)};
+    }
+
+    void Matrix4x4::SwapColumns(int col1, int col2) {
+        Swap(At(0, col1), At(0, col2));
+        Swap(At(1, col1), At(1, col2));
+        Swap(At(2, col1), At(2, col2));
+        Swap(At(3, col1), At(3, col2));
+    }
+
+    void Matrix4x4::SwapRows(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+        Swap(At(row1, 3), At(row2, 3));
+    }
+
+    void Matrix4x4::SwapRows3(int row1, int row2) {
+        Swap(At(row1, 0), At(row2, 0));
+        Swap(At(row1, 1), At(row2, 1));
+        Swap(At(row1, 2), At(row2, 2));
+    }
+
+    void Matrix4x4::Pivot() {
+        int rowIndex = 0;
+
+        for(int col = 0; col < Cols; ++col)
+        {
+            int greatest = rowIndex;
+
+            // find the rowIndex k with k >= 1 for which Mkj has the largest absolute value.
+            for(int i = rowIndex; i < Rows; ++i)
+                if (std::abs(At(i, col)) > std::abs(At(greatest, col)))
+                    greatest = i;
+
+            if (std::abs(At(greatest, col)) != 0)
+            {
+                if (rowIndex != greatest)
+                    SwapRows(rowIndex, greatest); // the greatest now in rowIndex
+
+                ScaleRow(rowIndex, 1.f/At(rowIndex, col));
+
+                for(int r = 0; r < Rows; ++r)
+                    if (r != rowIndex)
+                        SetRow(r, GetRow(r) - GetRow(rowIndex) * At(r, col));
+
+                ++rowIndex;
+            }
+        }
+    }
+
+    void Matrix4x4::ScaleColumn3(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleColumn(int col, float scalar) {
+        At(0, col) *= scalar;
+        At(1, col) *= scalar;
+        At(2, col) *= scalar;
+        At(3, col) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow3(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+    }
+
+    void Matrix4x4::ScaleRow(int row, float scalar) {
+        At(row, 0) *= scalar;
+        At(row, 1) *= scalar;
+        At(row, 2) *= scalar;
+        At(row, 3) *= scalar;
+    }
 }

src/J3ML/LinearAlgebra/Vector4.cpp

@@ -141,6 +141,15 @@ Vector4 Vector4::operator-(const Vector4& rhs) const
 
     }
 
+    Vector4 &Vector4::operator=(const Vector4 &rhs) {
+        x = rhs.x;
+        y = rhs.y;
+        z = rhs.z;
+        w = rhs.w;
+
+        return *this;
+    }
+
 
 }
 #pragma endregion