Merge pull request 'Add AABB v AABB & Triangle v Triangle intersection, and fix dot product' (#27) from Miuna/j3ml-fork:main into main

Reviewed-on: #27

.gitea/workflows/doccy.yaml

@@ -0,0 +1,25 @@
+name: Build Docs With Doxygen
+run-name: Building documentation for ${{ gitea.repository }}.
+on: [push]
+
+jobs:
+  Explore-Gitea-Actions:
+    runs-on: ubuntu-22.04
+    steps:
+      - run: echo "The job was automatically triggered by a ${{ gitea.event_name }} event."
+      - run: echo "This job is now running on a ${{ runner.os }} server hosted by Gitea!"
+      - run: echo "The name of your branch is ${{ gitea.ref }} and your repository is ${{ gitea.repository }}."
+      - name: Check out repository code
+        uses: actions/checkout@v3
+      - run: echo "The ${{ gitea.repository }} repository has been cloned to the runner."
+      - run: echo "The workflow is now ready to build your docs on the runner."
+      - run: echo "Copying SSH key for file transfer into runner."
+      - run: echo "${{ secrets.SSHKEY }}" > /id_rsa
+      - run: chmod 600 /id_rsa
+      - run: echo "Installing doxygen on runner."
+      - run: apt-get update && apt-get install -y doxygen
+      - run: echo "Building documentation."
+      - run: doxygen Doxyfile
+      - run: echo "Copying built documentation to doc site."
+      - run: scp -o "IdentitiesOnly=yes" -o "StrictHostKeyChecking=no" -i /id_rsa -P ${{ secrets.SSHPORT }} -r html ${{ secrets.SSHUSER }}@${{ secrets.SSHIP }}:/var/www/html/$(echo "${{ gitea.repository }}" | cut -f 2 -d '/')
+      - run: echo "This job's status is ${{ job.status }}."

README.md

@@ -1,17 +1,60 @@
 # Josh's 3D Math Library - J3ML
 
+
 Yet Another C++ Math Standard
 
-## Motivation
-This project was sparked by a desire to gain a deeper understanding into the math underlying computer graphics. While packages such as glm, eigen, etc. are amazing libraries, it removes the fun of having to learn it the hard way.
+J3ML is a "Modern C++" C++ library designed to provide comprehensive support for 3D mathematical operations commonly used in computer graphics, game development, physics simulations, and related fields. It offers a wide range of functionalities to simplify the implementation of complex mathematical operations in your projects.
+
+![Static Badge](https://img.shields.io/badge/Lit-Based-%20)
 
-## Use Cases
 ## Features
-### LinearAlgebra
-#### Vectors
-#### Matrices
-#### Conversion Types
-### Geometry
-## Samples
-## Bugs / Issues
-## Compilation
+
+* <b>Vector Operations:</b> Comprehensive support for 3D vector operations including addition, subtraction, scalar multiplication, dot product, cross product, normalization, and more. 
+* **Matrix Operations:** Efficient implementation of 3x3 and 4x4 matrices with support for common operations such as multiplication, transpose, determinant calculation, and inverse calculation.
+* **Quaternion Operations:** Quaternion manipulation functions including conversion to/from axis-angle representation, quaternion multiplication, normalization, and interpolation (slerp).
+* **Transformation Functions:** Functions for transforming points, vectors, and normals using matrices and quaternions.
+* **Geometric Types:** Support for geometric types such as points, lines, rays, planes, spheres, axis-aligned bounding boxes (AABB), and oriented bounding boxes (OBB).
+* **Algorithms:** Implementation of various algorithms including Gilbert-Johnson-Keerthi (GJK) algorithm for collision detection, random number generator, and more.
+* **Utility Functions:** Additional utilities such as conversion between degrees and radians, random number generation, and common constants.
+
+# Usage
+
+To use J3ML in your C++ project, simply include the necessary header files and link against the library. Here's a basic example of how to use the library to perform vector addition:
+
+
+```cpp
+
+#include <iostream>
+#include <j3ml/LinearAlgebra.h>
+
+int main() {
+    // Create two 3D vectors
+    Vector3 v1(1.0, 2.0, 3.0);
+    Vector3 v2(4.0, 5.0, 6.0);
+
+    // Perform vector addition
+    Vector3 result = v1 + v2;
+    
+    // Output the result
+    std::cout << "Result: " << result << std::endl;
+    
+    return 0;
+}
+```
+
+For more detailed usage instructions and examples, please refer to the documentation.
+# Documentation
+
+Documentation is automatically generated from latest commit and is hosted at https://doc.redacted.cc/j3ml .
+
+# Contributing
+
+Contributions to J3ML are welcome! If you find a bug, have a feature request, or would like to contribute code, please submit an issue or pull request to the GitHub repository.
+
+# License
+
+J3ML is licensed under the Public Domain. See the LICENSE file for details.
+
+# Acknowledgements
+
+J3ML is developed and maintained by Joshua O'Leary from Redacted Software and contributors. Special thanks to William J Tomasine II.

include/J3ML/Algorithm/SAT.h

@@ -0,0 +1,7 @@
+#pragma once
+
+
+namespace J3ML::Algorithm
+{
+
+}

include/J3ML/Geometry/AABB.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <optional>
+
 #include <J3ML/LinearAlgebra.h>
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/Geometry/Shape.h>
@@ -8,96 +10,199 @@
 
 namespace J3ML::Geometry
 {
-
-
     using namespace J3ML::LinearAlgebra;
     using J3ML::Algorithm::RNG;
-    // A 3D axis-aligned bounding box
-    // This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
-    // computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
-    // intersection queries.
-    // the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
-    // axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
-    // be arbitrarily oriented in the space with respect to each other.
-    // If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
 
+    /// @brief A 3D axis-aligned bounding box.
+    /// This data structure can be used to represent coarse bounds of objects, in situations where detailed triangle-level
+    /// computations can be avoided. In physics systems, bounding boxes are used as an efficient early-out test for geometry
+    /// intersection queries.
+    /// the 'Axis-aligned' part in the name means that the local axes of this bounding box are restricted to align with the
+    /// axes of the world space coordinate system. This makes computation involving AABB's very fast, since AABB's cannot
+    /// be arbitrarily oriented in the space with respect to each other.
+    /// If you need to represent a box in 3D space with arbitrary orientation, see the class OBB. */
     class AABB : public Shape {
     public:
+        /// Specifies the minimum extent of this AABB in the world space x, y and z axes.
         Vector3 minPoint;
+        /// Specifies the maximum extent of this AABB in the world space x, y and z axes. [similarOverload: minPoint]
         Vector3 maxPoint;
     public:
         static int NumFaces() { return 6; }
         static int NumEdges() { return 12; }
         static int NumVertices() { return 8; }
     public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members minPoint and maxPoint are undefined after creating a new AABB using this
+            default constructor. Remember to assign to them before use.
+            @see minPoint, maxPoint. */
         AABB();
+
+        /// Constructs this AABB by specifying the minimum and maximum extending corners of the box.
+        /** @see minPoint, maxPoint. */
         AABB(const Vector3& min, const Vector3& max);
 
+        /// Constructs this AABB to enclose the given OBB.
+        /** This constructor computes the optimal minimum volume AABB that encloses the given OBB.
+            @note Since an AABB cannot generally represent an OBB, this conversion is not exact, but the returned AABB
+                specifies a larger volume.
+            @see class OBB. */
+        explicit AABB(const OBB &obb);
+
+        /// Constructs this AABB to enclose the given Sphere.
+        /** @see class Sphere. */
+        explicit AABB(const Sphere &s);
+
         Vector3 HalfDiagonal() const { return HalfSize(); }
 
         static AABB FromCenterAndSize(const Vector3 &center, const Vector3 &size);
 
+
+        /// Returns the minimum world-space coordinate along the given axis.
         float MinX() const;
-        float MinY() const;
-        float MinZ() const;
+        float MinY() const; ///< [similarOverload: MinX]
+        float MinZ() const; ///< [similarOverload: MinX]
+
+        /// Returns the maximum world-space coordinate along the given axis.
         float MaxX() const;
-        float MaxY() const;
-        float MaxZ() const;
+        float MaxY() const; ///< [similarOverload: MaxX]
+        float MaxZ() const; ///< [similarOverload: MaxX]
 
         /// 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;
 
+        /// [similarOverload: Size]
+        /** Returns Size()/2.
+            @see Size(), HalfDiagonal(). */
         Vector3 HalfSize() const;
 
         /// 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;
 
+        /// Tests if this AABB is finite.
+        /** @return True if the member variables of this AABB are valid floats and do not contain NaNs or infs, and false otherwise.
+            @see IsDegenerate(), minPoint, maxPoint. */
         bool IsFinite() const;
 
+        /// @return The center point of this AABB.
         Vector3 Centroid() const;
 
+        /// Returns the side lengths of this AABB in x, y and z directions.
+        /** The returned vector is equal to the diagonal vector of this AABB, i.e. it spans from the
+            minimum corner of the AABB to the maximum corner of the AABB.
+            @see HalfSize(), Diagonal(). */
         Vector3 Size() const;
 
         // Quickly returns an arbitrary point inside this AABB
         Vector3 AnyPointFast() const;
 
+        /// Generates a point inside this AABB.
+        /** @param x A normalized value between [0,1]. This specifies the point position along the world x axis.
+            @param y A normalized value between [0,1]. This specifies the point position along the world y axis.
+            @param z A normalized value between [0,1]. This specifies the point position along the world z axis.
+            @return A point inside this AABB at point specified by given parameters.
+            @see Edge(), CornerPoint(), PointOnEdge(), FaceCenterPoint(), FacePoint(). */
         Vector3 PointInside(float x, float y, float z) const;
 
         // Returns an edge of this AABB
         LineSegment Edge(int edgeIndex) const;
 
+        /// Returns a corner point of this AABB.
+        /** This function generates one of the eight corner points of this AABB.
+            @param cornerIndex The index of the corner point to generate, in the range [0, 7].
+                The points are returned in the order 0: ---, 1: --+, 2: -+-, 3: -++, 4: +--, 5: +-+, 6: ++-, 7: +++. (corresponding the XYZ axis directions).
+            @todo Draw which index generates which corner point.
+            @see PointInside(), Edge(), PointOnEdge(), FaceCenterPoint(), FacePoint(), GetCornerPoints(). */
         Vector3 CornerPoint(int cornerIndex) const;
 
+        /// Computes an extreme point of this AABB in the given direction.
+        /** An extreme point is a farthest point of this AABB in the given direction. Given a direction,
+            this point is not necessarily unique.
+            @param direction The direction vector of the direction to find the extreme point. This vector may
+                be unnormalized, but may not be null.
+            @return An extreme point of this AABB in the given direction. The returned point is always a
+                corner point of this AABB.
+            @see CornerPoint(). */
         Vector3 ExtremePoint(const Vector3 &direction) const;
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
 
+        /// Returns a point on an edge of this AABB.
+        /** @param edgeIndex The index of the edge to generate a point to, in the range [0, 11]. @todo Document which index generates which one.
+            @param u A normalized value between [0,1]. This specifies the relative distance of the point along the edge.
+            @see PointInside(), CornerPoint(), CornerPoint(), FaceCenterPoint(), FacePoint(). */
         Vector3 PointOnEdge(int edgeIndex, float u) const;
 
+        /// Returns the point at the center of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @see PointInside(), CornerPoint(), PointOnEdge(), PointOnEdge(), FacePoint(). */
         Vector3 FaceCenterPoint(int faceIndex) const;
 
+        /// Generates a point at the surface of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @param u A normalized value between [0, 1].
+            @param v A normalized value between [0, 1].
+            @see PointInside(), CornerPoint(), PointOnEdge(), PointOnEdge(), FaceCenterPoint(). */
         Vector3 FacePoint(int faceIndex, float u, float v) const;
 
+        /// Returns the surface normal direction vector the given face points towards.
+        /** @param faceIndex The index of the AABB face to generate the point at. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @see FacePoint(), FacePlane(). */
         Vector3 FaceNormal(int faceIndex) const;
 
+        /// Computes the plane equation of the given face of this AABB.
+        /** @param faceIndex The index of the AABB face. The valid range is [0, 5].
+                This index corresponds to the planes in the order (-X, +X, -Y, +Y, -Z, +Z).
+            @return The plane equation the specified face lies on. The normal of this plane points outwards from this AABB.
+            @see FacePoint(), FaceNormal(), GetFacePlanes(). */
         Plane FacePlane(int faceIndex) const;
 
+
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
+        /// Generates an AABB that encloses the given point set.
+        /** This function finds the smallest AABB that contains the given set of points.
+            @param pointArray A pointer to an array of points to enclose inside an AABB.
+            @param numPoints The number of elements in the pointArray list.
+            @see SetFrom(). */
         static AABB MinimalEnclosingAABB(const Vector3 *pointArray, int numPoints);
-        float GetVolume() const;
-        float GetSurfaceArea() const;
+        AABB MinimalEnclosingAABB() const { return *this;}
+        /// Computes the volume of this AABB.
+        /** @see SurfaceArea(), IsDegenerate(). */
+        float Volume() const;
+        /// Computes the surface area of the faces of this AABB.
+        /** @see Volume(). */
+        float SurfaceArea() const;
         Vector3 GetClosestPoint(const Vector3& point) const;
 
         void Translate(const Vector3& offset);
         AABB Translated(const Vector3& offset) const;
+        void Scale(const Vector3& scale);
+        AABB Scaled(const Vector3& scale) const;
         AABB TransformAABB(const Matrix3x3& transform);
         AABB TransformAABB(const Matrix4x4& transform);
         AABB TransformAABB(const Quaternion& transform);
+        /// Applies a transformation to this AABB and returns the resulting OBB.
+        /** Transforming an AABB produces an oriented bounding box. This set of functions does not apply the transformation
+            to this object itself, but instead returns the OBB that results in the transformation.
+            @param transform The transformation to apply to this AABB. This function assumes that this
+                transformation does not contain shear, nonuniform scaling or perspective properties, i.e. that the fourth
+                row of the float4x4 is [0 0 0 1].
+            @see Translate(), Scale(), TransformAsAABB(), classes float3x3, float3x4, float4x4, Quat. */
         OBB Transform(const Matrix3x3& transform) const;
         OBB Transform(const Matrix4x4& transform) const;
         OBB Transform(const Quaternion& transform) const;
+
+        /// Tests if the given object is fully contained inside this AABB.
+        /** This function returns true if the given object lies inside this AABB, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this AABB count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
         bool Contains(const Vector3& point) const;
         bool Contains(const Vector3& aabbMinPoint, const Vector3& aabbMaxPoint) const;
         bool Contains(const LineSegment& lineSegment) const;
@@ -109,27 +214,94 @@ namespace J3ML::Geometry
         bool Contains(const Frustum& frustum) const;
         bool Contains(const Polyhedron& polyhedron) const;
         bool Contains(const Capsule& capsule) const;
-        // Tests whether this AABB and the given object intersect.
+
+        /// Tests whether this AABB and the given object intersect.
+        /** Both objects are treated as "solid", meaning that if one of the objects is fully contained inside
+            another, this function still returns true. (e.g. in case a line segment is contained inside this AABB,
+            or this AABB is contained inside a Sphere, etc.)
+            @param ray The first parameter of this function specifies the other object to test against.
+            @param dNear [out] If specified, receives the parametric distance along the line denoting where the
+                line entered this AABB.
+            @param dFar [out] If specified, receives the parametric distance along the line denoting where the
+                line exited this AABB.
+            @see Contains(), Distance(), ClosestPoint().
+            @note If you do not need the intersection intervals, you should call the functions without these
+                parameters in the function signature for optimal performance.
+            @todo Add Intersects(Circle/Disc). */
         bool Intersects(const Ray& ray, float dNear, float dFar) const;
         bool Intersects(const Capsule& capsule) const;
         bool Intersects(const Triangle& triangle) const;
         bool Intersects(const Polygon& polygon) const;
         bool Intersects(const Frustum& frustum) const;
         bool Intersects(const Polyhedron& polyhedron) const;
+        bool Intersects(const AABB& aabb) const;
+
+        /** For reference documentation on the Sphere-AABB intersection test, see Christer Ericson's Real-Time Collision Detection, p. 165. [groupSyntax]
+		@param sphere The first parameter of this function specifies the other object to test against.
+		@param closestPointOnAABB [out] Returns the closest point on this AABB to the given sphere. This pointer
+			may be null. */
+        bool Intersects(const Sphere &sphere, Vector3 *closestPointOnAABB = 0) const;
+
+        /// Generates an unindexed triangle mesh representation of this AABB.
+        /** @param numFacesX The number of faces to generate along the X axis. This value must be >= 1.
+            @param numFacesY The number of faces to generate along the Y axis. This value must be >= 1.
+            @param numFacesZ The number of faces to generate along the Z axis. This value must be >= 1.
+            @param outPos [out] An array of size numVertices which will receive a triangle list
+                of vertex positions. Cannot be null.
+            @param outNormal [out] An array of size numVertices which will receive vertex normals.
+                If this parameter is null, vertex normals are not returned.
+            @param outUV [out] An array of size numVertices which will receive vertex UV coordinates.
+                If this parameter is null, a UV mapping is not generated.
+            @param ccwIsFrontFacing If true, then the front-facing direction of the faces will be the sides
+                with counterclockwise winding order. Otherwise, the faces are generated in clockwise winding order.
+            The number of vertices that outPos, outNormal and outUV must be able to contain is
+            (x*y + x*z + y*z)*2*6. If x==y==z==1, then a total of 36 vertices are required. Call
+            NumVerticesInTriangulation to obtain this value.
+            @see ToPolyhedron(), ToEdgeList(), NumVerticesInTriangulation(). */
         TriangleMesh Triangulate(int numFacesX, int numFacesY, int numFacesZ, bool ccwIsFrontFacing) const;
 
+        /// Returns the number of vertices that the Triangulate() function will output with the given subdivision parameters.
+        /** @see Triangulate(). */
+        static int NumVerticesInTriangulation(int numFacesX, int numFacesY, int numFacesZ)
+        {
+            return (numFacesX*numFacesY + numFacesX*numFacesZ + numFacesY*numFacesZ)*2*6;
+        }
+
+        /// Returns the number of vertices that the ToEdgeList() function will output.
+        /** @see ToEdgeList(). */
+        static int NumVerticesInEdgeList()
+        {
+            return 4*3*2;
+        }
+
         /// Finds the set intersection of this and the given AABB.
-        /** @return This function returns the AABB that is contained in both this and the given AABB.
+        /** @return This function returns an intersection that is contained in both this and the given AABB if there is one.
             @todo Add Intersection(OBB/Polyhedron). */
-        AABB Intersection(const AABB& rhs) const;
+        std::optional<AABB> Intersection(const AABB& rhs) const;
 
 
+        /// Sets this AABB to enclose the given set of points.
+        /** @param pointArray A pointer to an array of points to enclose inside an AABB.
+            @param numPoints The number of elements in the pointArray list.
+            @see MinimalEnclosingAABB(). */
         void SetFrom(const Vector3 *pVector3, int i);
 
+        /// Sets this AABB by specifying its center and size.
+        /** @param center The center point of this AABB.
+            @param size A vector that specifies the size of this AABB in x, y and z directions.
+            @see SetFrom(), FromCenterAndSize(). */
         void SetFromCenterAndSize(const Vector3 &center, const Vector3 &size);
 
+        /// Sets this AABB to enclose the given OBB.
+        /** This function computes the minimal axis-aligned bounding box for the given oriented bounding box. If the orientation
+            of the OBB is not aligned with the world axes, this conversion is not exact and loosens the volume of the bounding box.
+            @param obb The oriented bounding box to convert into this AABB.
+            @todo Implement SetFrom(Polyhedron).
+            @see SetCenter(), class OBB. */
         void SetFrom(const OBB &obb);
 
+        /// Sets this AABB to enclose the given sphere.
+        /** This function computes the smallest possible AABB (in terms of volume) that contains the given sphere, and stores the result in this structure. */
         void SetFrom(const Sphere &s);
 
         Vector3 GetRandomPointInside(RNG& rng) const;
@@ -137,15 +309,28 @@ namespace J3ML::Geometry
         Vector3 GetRandomPointOnEdge(RNG& rng) const;
         Vector3 GetRandomCornerPoint(RNG& rng) const;
 
+        /// Sets this structure to a degenerate AABB that does not have any volume.
+        /** This function is useful for initializing the AABB to "null" before a loop of calls to Enclose(),
+            which incrementally expands the bounds of this AABB to enclose the given objects.
+            @see Enclose(). */
         void SetNegativeInfinity();
 
+        /// Expands this AABB to enclose the given object.
+        /** This function computes an AABB that encloses both this AABB and the specified object, and stores the resulting
+            AABB into this.
+            @note The generated AABB is not necessarily the optimal enclosing AABB for this AABB and the given object. */
         void Enclose(const Vector3 &point);
-
         void Enclose(const Vector3 &aabbMinPt, const Vector3 &aabbMaxPt);
-
         void Enclose(const LineSegment &lineSegment);
-
         void Enclose(const OBB &obb);
+        void Enclose(const Sphere &sphere);
+        void Enclose(const Triangle &triangle);
+        void Enclose(const Capsule &capsule);
+        void Enclose(const Frustum &frustum);
+        void Enclose(const Polygon &polygon);
+        void Enclose(const Polyhedron &polyhedron);
+        void Enclose(const Vector3 *pointArray, int numPoints);
+
 
         bool TestAxis(const Vector3& axis, const Vector3& v0, const Vector3& v1, const Vector3& v2) const;
 

include/J3ML/Geometry/Capsule.h

@@ -2,7 +2,7 @@
 
 #include "LineSegment.h"
 #include "Shape.h"
-#include <J3ML/LinearAlgebra/Vector3.h>
+#include <J3ML/LinearAlgebra.h>
 #include <J3ML/Geometry/Common.h>
 
 namespace J3ML::Geometry
@@ -16,7 +16,7 @@ namespace J3ML::Geometry
         b = std::move(temp);
     }
 
-    using namespace LinearAlgebra;
+    /// A 3D cylinder with spherical ends.
     class Capsule : public Shape
     {
     public:
@@ -25,13 +25,39 @@ namespace J3ML::Geometry
         // Specifies the radius of this capsule
         float r;
     public:
+        /// The default constructor does not initialize any members of this class.
+        /** This means that the values of the members l and r are both undefined after creating a new capsule using
+            this default constructor. Remember to assign to them before use.
+            @see l, r. */
         Capsule();
+        /// Constructs a new capsule by explicitly specifying the member variables.
+        /** @param endPoints Specifies the line segment of the capsule.
+            @param radius Specifies the size of this capsule.
+            @see l, r. */
         Capsule(const LineSegment& endPoints, float radius);
+        /// Constructs a new capsule by explicitly specifying the member variables.
+        /** This constructor is equivalent to calling Capsule(LineSegment(bottomPoint, topPoint), radius), but provided
+            here for conveniency.
+            @see l, r. */
         Capsule(const Vector3& bottomPt, const Vector3& topPt, float radius);
 
         /// Quickly returns an arbitrary point inside this Capsule. Used in GJK intersection test.
         inline Vector3 AnyPointFast() const { return l.A; }
 
+        /// Generates a point that perhaps lies inside this capsule.
+        /** @param height A normalized value between [0,1]. This specifies the point position along the height line of this capsule.
+            @param x A normalized value between [0,1]. This specifies the x coordinate on the plane of the circle cross-section specified by l.
+            @param y A normalized value between [0,1]. This specifies the y coordinate on the plane of the circle cross-section specified by l.
+            @note This function will generate points uniformly, but they do not necessarily lie inside the capsule.
+            @see PointInside(). */
+        Vector3 UniformPointPerhapsInside(float height, float x, float y) const;
+
+        /// Returns the Sphere defining the 'bottom' section of this Capsule (corresponding to the endpoint l.a)
+        Sphere SphereA() const;
+
+        /// Returns the Sphere defining the 'top' section of this Capsule (corresponding to the endpoint l.b)
+        Sphere SphereB() const;
+
         /// Computes the extreme point of this Capsule in the given direction.
         /** An extreme point is a farthest point of this Capsule in the given direction. Given a direction,
             this point is not necessarily unique.
@@ -40,40 +66,135 @@ namespace J3ML::Geometry
             @return The extreme point of this Capsule in the given direction. */
         Vector3 ExtremePoint(const Vector3 &direction) const;
         Vector3 ExtremePoint(const Vector3 &direction, float &projectionDistance) const;
-
+        /// Tests if this Capsule is degenerate.
+        /** @return True if this Capsule does not span a strictly positive volume. */
         bool IsDegenerate() const;
+        /// Computes the total height of this capsule, i.e. LineLength() + Diameter().
+        /** <img src="CapsuleFunctions.png" />
+            @see LineLength(). */
         float Height() const;
+        /// Computes the diameter of this capsule.
         float Diameter() const;
+        /// Returns the bottom-most point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @note The bottom-most point is only a naming convention, and does not correspond to the bottom-most point along any world axis. The returned
+                point is simply the point at the far end of this Capsule where the point l.a resides.
+            @note The bottom-most point of the capsule is different than the point l.a. The returned point is the point at the very far
+                edge of this capsule, and does not lie on the internal line. See the attached diagram.
+            @see Top(), l. */
         Vector3 Bottom() const;
+        /// Returns the center point of this Capsule.
+        /** <img src="doc/static/docs/CapsuleFunctions.png" />
+            @return The point (l.a + l.b) / 2. This point is the center of mass for this capsule.
+            @see l, Bottom(), Top(). */
         Vector3 Center() const;
-        Vector3 Centroid() const;
+        Vector3 Centroid() const; ///< [similarOverload: Center]
+
+        /// Returns the direction from the bottommost point towards the topmost point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @return The normalized direction vector from l.a to l.b.
+            @see l. */
+        Vector3 UpDirection() const;
+
+        /// Computes the volume of this Capsule.
+        /** @return pi * r^2 * |b-a| + 4 * pi * r^2 / 3.
+            @see SurfaceArea(). */
+        float Volume() const;
+
+        /// Computes the surface area of this Capsule.
+        /** @return 2 * pi * r * |b-a| + 4 * pi * r^2.
+            @see Volume(). */
+        float SurfaceArea() const;
+
+        /// Returns the cross-section circle at the given height of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @param l A normalized parameter between [0,1]. l == 0 returns a degenerate circle of radius 0 at the bottom of this Capsule, and l == 1
+                    will return a degenerate circle of radius 0 at the top of this Capsule. */
+        //Circle CrossSection(float l) const;
+
         Vector3 ExtremePoint(const Vector3& direction);
+
+        /// Returns the smallest AABB that encloses this capsule.
+        /** @see MinimalEnclosingOBB(). */
         AABB MinimalEnclosingAABB() const;
 
+        /// Returns the smallest OBB that encloses this capsule.
+        /** @see MinimalEnclosingAABB(). */
+        OBB MinimalEnclosingOBB() const;
+
+        /// Projects this Capsule onto the given 1D axis direction vector.
+        /** This function collapses this Capsule onto an 1D axis for the purposes of e.g. separate axis test computations.
+            The function returns a 1D range [outMin, outMax] denoting the interval of the projection.
+            @param direction The 1D axis to project to. This vector may be unnormalized, in which case the output
+                of this function gets scaled by the length of this vector.
+            @param outMin [out] Returns the minimum extent of this object along the projection axis.
+            @param outMax [out] Returns the maximum extent of this object along the projection axis. */
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
+        /// Returns the topmost point of this Capsule.
+        /** <img src="CapsuleFunctions.png" />
+            @note The topmost point is only a naming convention, and does not correspond to the topmost point along any world axis. The returned
+                point is simply the point at the far end of this Capsule where the point l.b resides.
+            @note The topmost point of the capsule is different than the point l.b. The returned point is the point at the very far
+                edge of this capsule, and does not lie on the internal line. See the attached diagram.
+            @see Bottom(), l. */
+        Vector3 Top() const;
+
+        /// Applies a transformation to this capsule.
+        /** @param transform The transformation to apply to this capsule. This transformation must be
+            affine, and must contain an orthogonal set of column vectors (may not contain shear or projection).
+            The transformation can only contain uniform scale, and may not contain mirroring.
+            @see Translate(), Scale(), classes Matrix3x3, Matrix4x4, Quaternion. */
+        void Transform(const Matrix3x3 &transform);
+        void Transform(const Matrix4x4 &transform);
+        void Transform(const Quaternion &transform);
+
+        /// Computes the closest point inside this capsule to the given point.
+        /** If the target point lies inside this capsule, then that point is returned.
+            @see Distance(), Contains(), Intersects().
+            @todo Add ClosestPoint(Line/Ray/LineSegment/Plane/Triangle/Polygon/Circle/Disc/AABB/OBB/Sphere/Capsule/Frustum/Polyhedron). */
+        Vector3 ClosestPoint(const Vector3 &targetPoint) const;
+
+
+        /// Computes the distance between this capsule and the given object.
+        /** This function finds the nearest pair of points on this and the given object, and computes their distance.
+            If the two objects intersect, or one object is contained inside the other, the returned distance is zero.
+            @todo Add Distance(Triangle/Polygon/Circle/Disc/Capsule).
+            @see Contains(), Intersects(), ClosestPoint(). */
+        float Distance(const Vector3 &point) const;
+        float Distance(const Plane &plane) const;
+        float Distance(const Sphere &sphere) const;
+        float Distance(const Ray &ray) const;
+        float Distance(const Line &line) const;
+        float Distance(const LineSegment &lineSegment) const;
+        float Distance(const Capsule &capsule) const;
+
+        /// Tests if the given object is fully contained inside this capsule.
+        /** This function returns true if the given object lies inside this capsule, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the surface of this capsule count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
+        bool Contains(const Vector3 &point) const;
+        bool Contains(const LineSegment &lineSegment) const;
+        bool Contains(const Triangle &triangle) const;
+        bool Contains(const Polygon &polygon) const;
+        bool Contains(const AABB &aabb) const;
+        bool Contains(const OBB &obb) const;
+        bool Contains(const Frustum &frustum) const;
+        bool Contains(const Polyhedron &polyhedron) const;
+
         bool Intersects(const Plane &plane) const;
-
         bool Intersects(const Ray &ray) const;
-
         bool Intersects(const Line &line) const;
-
         bool Intersects(const LineSegment &lineSegment) const;
-
         bool Intersects(const AABB &aabb) const;
-
         bool Intersects(const OBB &obb) const;
-
         bool Intersects(const Sphere &sphere) const;
-
         bool Intersects(const Capsule &capsule) const;
-
         bool Intersects(const Triangle &triangle) const;
-
         bool Intersects(const Polygon &polygon) const;
-
         bool Intersects(const Frustum &frustum) const;
-
         bool Intersects(const Polyhedron &polyhedron) const;
     };
 }

include/J3ML/Geometry/Common.h

@@ -27,5 +27,13 @@ namespace J3ML::Geometry
 // Methods required by Geometry types
 namespace J3ML::Geometry
 {
+    // Represents a segment along an axis, with the axis as a unit
+    struct Interval {
+        float min;
+        float max;
 
+        bool Intersects(const Interval& rhs) const;
+
+        bool operator==(const Interval& rhs) const = default;
+    };
 }

include/J3ML/Geometry/Frustum.h

@@ -51,7 +51,7 @@ namespace J3ML::Geometry
         D3D,
     };
 
-    /// The handedness rule in J3ML bundles together two different conventions related to the camera:
+    /// @brief The handedness rule in J3ML bundles together two different conventions related to the camera:
     /// * the chirality of the world and view spaces,
     /// * the fixed local front direction of the Frustum.
     /// @note The world and view spaces are always assumed to the same chirality, meaning that Frustum::ViewMatrix()
@@ -74,10 +74,12 @@ namespace J3ML::Geometry
         Right
     };
 
-    /// Represents either an orthographic or a perspective viewing frustum.
+    /// @brief Represents either an orthographic or a perspective viewing frustum.
+    /// @see FrustumType
+    /// @see FrustumProjectiveSpace
+    /// @see FrustumHandedness
     class Frustum : public Shape {
-    public: /// Members
-
+    public: // Members
 
         /// Specifies whether this frustum is a perspective or an orthographic frustum.
         FrustumType type;
@@ -138,85 +140,228 @@ namespace J3ML::Geometry
         Matrix4x4 worldMatrix;
         Matrix4x4 projectionMatrix;
         Matrix4x4 viewProjectionMatrix;
-    public: /// Methods
-        Frustum()
-        : type(FrustumType::Invalid),
-        pos(Vector3::NaN),
-        front(Vector3::NaN),
-        up(Vector3::NaN),
-        nearPlaneDistance(NAN),
-        farPlaneDistance(NAN),
-        worldMatrix(Matrix4x4::NaN),
-        viewProjectionMatrix(Matrix4x4::NaN)
-        {
-            // For conveniency, allow automatic initialization of the graphics API and handedness in use.
-            // If neither of the #defines are set, user must specify per-instance.
-        }
+    public:
 
+    /// The default constructor creates an uninitialized Frustum object.
+    /** This means that the values of the members type, projectiveSpace, handedness, pos, front, up, nearPlaneDistance, farPlaneDistance, horizontalFov/orthographicWidth and
+    verticalFov/orthographicHeight are all NaN after creating a new Frustum using this
+    default constructor. Remember to assign to them before use.
+    @note As an exception to other classes in MathGeoLib, this class initializes its members to NaNs, whereas the other classes leave the members uninitialized. This difference
+        is because the Frustum class implements a caching mechanism where world, projection and viewProj matrices are recomputed on demand, which does not work nicely together
+        if the defaults were uninitialized.
+        */
+        Frustum();
 
         /// Quickly returns an arbitrary point inside this Frustum. Used in GJK intersection test.
         inline Vector3 AnyPointFast() const { return CornerPoint(0); }
 
         static Frustum CreateFrustumFromCamera(const CoordinateFrame& cam, float aspect, float fovY, float zNear, float zFar);
+        /// Returns the tightest AABB that contains this Frustum.
+        /** This function computes the optimal minimum volume AABB that encloses this Frustum.
+            @note Since an AABB cannot generally represent a Frustum, this conversion is not exact, but the returned AABB
+                specifies a larger volume.
+            @see MinimalEnclosingOBB(), ToPolyhedron(). */
         AABB MinimalEnclosingAABB() const;
+        /// Returns the tightest OBB that encloses this Frustum.
+        /** This function computes the optimal minimum volume OBB that encloses this Frustum.
+            @note If the type of this frustum is Perspective, this conversion is not exact, but the returned OBB specifies
+                a larger volume. If the type of this Frustum is orthographic, this conversion is exact, since the shape of an
+                orthographic Frustum is an OBB.
+            @see MinimalEnclosingAABB(), ToPolyhedron(). */
         OBB MinimalEnclosingOBB() const;
+        /// Sets the type of this Frustum.
+        /** @note Calling this function recomputes the cached view and projection matrices of this Frustum.
+            @see SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), ProjectiveSpace(), Handedness(). */
         void SetKind(FrustumProjectiveSpace projectiveSpace, FrustumHandedness handedness);
+        /// Sets the depth clip distances of this Frustum.
+        /** @param nearPlaneDistance The z distance from the eye point to the position of the Frustum near clip plane. Always pass a positive value here.
+            @param farPlaneDistance The z distance from the eye point to the position of the Frustum far clip plane. Always pass a value that is larger than nearClipDistance.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetFrame(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), NearPlaneDistance(), FarPlaneDistance(). */
         void SetViewPlaneDistances(float nearPlaneDistance, float farPlaneDistance);
+        /// Specifies the full coordinate space of this Frustum in one call.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @note As a micro-optimization, prefer this function over the individual SetPos/SetFront/SetUp functions if you need to do a batch of two or more changes, to avoid
+            redundant recomputation of the world matrix.
+            @see SetKind(), SetViewPlaneDistances(), SetPos(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), Pos(), Front(), Up(). */
         void SetFrame(const Vector3& pos, const Vector3& front, const Vector3& up);
+        /// Sets the world-space position of this Frustum.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetFront(), SetUp(), SetPerspective(), SetOrthographic(), Pos(). */
         void SetPos(const Vector3& pos);
+        /// Sets the world-space direction the Frustum eye is looking towards.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetUp(), SetPerspective(), SetOrthographic(), Front(). */
         void SetFront(const Vector3& front);
+        /// Sets the world-space camera up direction vector of this Frustum.
+        /** @note Calling this function recomputes the cached world matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetPerspective(), SetOrthographic(), Up(). */
         void SetUp(const Vector3& up);
+        /// Makes this Frustum use a perspective projection formula with the given FOV parameters.
+        /** A Frustum that uses the perspective projection is shaped like a pyramid that is cut from the top, and has a
+            base with a rectangular area.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetOrthographic(), HorizontalFov(), VerticalFov(), SetHorizontalFovAndAspectRatio(), SetVerticalFovAndAspectRatio(). */
         void SetPerspective(float horizontalFov, float verticalFov);
+        /// Makes this Frustum use an orthographic projection formula with the given FOV parameters.
+        /** A Frustum that uses the orthographic projection is shaded like a cube (an OBB).
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetKind(), SetViewPlaneDistances(), SetFrame(), SetPos(), SetFront(), SetUp(), SetOrthographic(), OrthographicWidth(), OrthographicHeight(). */
         void SetOrthographic(float orthographicWidth, float orthographicHeight);
+        /// Returns the handedness of the projection formula used by this Frustum.
+        /** @see SetKind(), FrustumHandedness. */
         FrustumHandedness Handedness() const { return handedness; }
+        /// Returns the type of the projection formula used by this Frustum.
+        /** @see SetPerspective(), SetOrthographic(), FrustumType. */
         FrustumType Type() const { return type; }
+        /// Returns the convention of the post-projective space used by this Frustum.
+        /** @see SetKind(), FrustumProjectiveSpace. */
         FrustumProjectiveSpace ProjectiveSpace() const { return projectiveSpace;}
+        /// Returns the world-space position of this Frustum.
+        /** @see SetPos(), Front(), Up(). */
         const Vector3 &Pos() const {return pos;}
+        /// Returns the world-space camera look-at direction of this Frustum.
+        /** @see Pos(), SetFront(), Up(). */
         const Vector3 &Front() const { return front; }
+        /// Returns the world-space camera up direction of this Frustum.
+        /** @see Pos(), Front(), SetUp(). */
         const Vector3 &Up() const { return up; }
+        /// Returns the distance from the Frustum eye to the near clip plane.
+        /** @see SetViewPlaneDistances(), FarPlaneDistance(). */
         float NearPlaneDistance() const { return nearPlaneDistance; }
+        /// Returns the distance from the Frustum eye to the far clip plane.
+        /** @see SetViewPlaneDistances(), NearPlaneDistance(). */
         float FarPlaneDistance() const { return farPlaneDistance;}
+        /// Returns the horizontal field-of-view used by this Frustum, in radians.
+        /** @note Calling this function when the Frustum is not set to use perspective projection will return values that are meaningless.
+            @see SetPerspective(), Type(), VerticalFov(). */
         float HorizontalFov() const { return horizontalFov;}
+        /// Returns the vertical field-of-view used by this Frustum, in radians.
+        /** @note Calling this function when the Frustum is not set to use perspective projection will return values that are meaningless.
+            @see SetPerspective(), Type(), HorizontalFov(). */
         float VerticalFov() const { return verticalFov;}
+        /// Returns the world-space width of this Frustum.
+        /** @note Calling this function when the Frustum is not set to use orthographic projection will return values that are meaningless.
+            @see SetOrthographic(), Type(), OrthographicHeight(). */
         float OrthographicWidth() const { return orthographicWidth; }
+        /// Returns the world-space height of this Frustum.
+        /** @note Calling this function when the Frustum is not set to use orthographic projection will return values that are meaningless.
+            @see SetOrthographic(), Type(), OrthographicWidth(). */
         float OrthograhpicHeight() const { return orthographicHeight; }
+        /// Returns the number of line segment edges that this Frustum is made up of, which is always 12.
+        /** This function is used in template-based algorithms to provide an unified API for iterating over the features of a Polyhedron. */
         int NumEdges() const { return 12; }
+        /// Returns the aspect ratio of the view rectangle on the near plane.
+        /** The aspect ratio is the ratio of the width of the viewing rectangle to its height. This can also be computed by
+            the expression horizontalFov / verticalFov. To produce a proper non-stretched image when rendering, this
+            aspect ratio should match the aspect ratio of the actual render target (e.g. 4:3, 16:9 or 16:10 in full screen mode).
+            @see horizontalFov, verticalFov. */
         float AspectRatio() const;
-
+        /// Makes this Frustum use a perspective projection formula with the given horizontal FOV parameter and aspect ratio.
+        /** Specifies the horizontal and vertical field-of-view values for this Frustum based on the given horizontal FOV
+            and the screen size aspect ratio.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetPerspective(), SetVerticalFovAndAspectRatio(). */
         void SetHorizontalFovAndAspectRatio(float horizontalFov, float aspectRatio);
-
+        /// Makes this Frustum use a perspective projection formula with the given vertical FOV parameter and aspect ratio.
+        /** Specifies the horizontal and vertical field-of-view values for this Frustum based on the given vertical FOV
+            and the screen size aspect ratio.
+            @note Calling this function recomputes the cached projection matrix of this Frustum.
+            @see SetPerspective(), SetHorizontalFovAndAspectRatio(). */
+        void SetVerticalFovAndAspectRatio(float verticalFov, float aspectRatio);
 
         Vector3 CornerPoint(int cornerIndex) const;
 
         Vector3 NearPlanePos(float x, float y) const;
         Vector3 FarPlanePos(float x, float y) const;
 
-        Vector3 WorldRight() const
-        {
-            if (handedness == FrustumHandedness::Right)
-                return Vector3::Cross(front, up);
-            else
-                return Vector3::Cross(up, front);
-        }
+        /// Computes the direction vector that points logically to the right-hand side of the Frustum.
+        /** This vector together with the member variables 'front' and 'up' form the orthonormal basis of the view frustum.
+            @see pos, front. */
+        Vector3 WorldRight() const;
 
-        Plane TopPlane() const;
-        Plane BottomPlane() const;
-        Plane RightPlane() const;
+        Plane TopPlane() const;     ///< [similarOverload: LeftPlane] [hideIndex]
+        Plane BottomPlane() const;  ///< [similarOverload: LeftPlane] [hideIndex]
+        Plane RightPlane() const;   ///< [similarOverload: LeftPlane] [hideIndex]
+        /// Returns the plane equation of the specified side of this Frustum.
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum.
+            This means the negative half-space of the Frustum is the space inside the Frustum.
+            [indexTitle: Left/Right/Top/BottomPlane]
+            @see NearPlane(), FarPlane(), GetPlane(), GetPlanes(). */
         Plane LeftPlane() const;
+        /// Computes the plane equation of the far plane of this Frustum. [similarOverload: NearPlane]
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum, i.e. away from the eye point.
+            (towards front). This means the negative half-space of the Frustum is the space inside the Frustum.
+            @see front, FarPlane(), LeftPlane(), RightPlane(), TopPlane(), BottomPlane(), GetPlane(), GetPlanes(). */
         Plane FarPlane() const;
+        /// Computes the plane equation of the near plane of this Frustum.
+        /** The normal vector of the returned plane points outwards from the volume inside the frustum, i.e. towards the eye point
+            (towards -front). This means the negative half-space of the Frustum is the space inside the Frustum.
+            @see front, FarPlane(), LeftPlane(), RightPlane(), TopPlane(), BottomPlane(), GetPlane(), GetPlanes(). */
         Plane NearPlane() const;
+        /// Computes the width of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
         float NearPlaneWidth() const;
+        /// Computes the height of the near plane quad in world space units.
+        /** @see NearPlaneHeight(). */
         float NearPlaneHeight() const;
 
-
+        /// Moves this Frustum by the given offset vector.
+        /** @note This function operates in-place.
+            @param offset The world space offset to apply to the position of this Frustum.
+            @see Transform(). */
         void Translate(const Vector3& offset);
+        /// Applies a transformation to this Frustum.
+        /** @param transform The transformation to apply to this Frustum. This transformation must be
+         * affine, and must contain an orthogoal set of column vectors (may not contain shear or projection).
+         * The transformation can only contain uniform
+         * @see Translate(), Scale(), classes Matrix3x3, Matrix4x4, Quaternion
+         */
         void Transform(const Matrix3x3& transform);
         void Transform(const Matrix4x4& transform);
         void Transform(const Quaternion& transform);
 
-
+        /// Converts this Frustum to a Polyhedron.
+        /** This function returns a Polyhedron representation of this Frustum. This conversion is exact, meaning that the returned
+            Polyhedron represents exactly the same set of points that this Frustum does.
+            @see MinimalEnclosingAABB(), MinimalEnclosingOBB(). */
         Polyhedron ToPolyhedron() const;
 
+        /// Converts this Frustum to a PBVolume.
+        /** This function returns a plane-bounded volume representation of this Frustum. The conversion is exact, meaning that the
+            returned PBVolume<6> represents exactly the same set of points that this Frustum does.
+            @see ToPolyhedron(). */
+        //PBVolume<6> ToPBVolume() const;
+
+        /// Tests if the given object is fully contained inside this Frustum.
+        /** This function returns true if the given object lies inside this Frustum, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this Frustum count as being inside, but
+                due to float inaccuracies, this cannot generally be relied upon.
+            @todo Add Contains(Circle/Disc/Sphere/Capsule).
+            @see Distance(), Intersects(), ClosestPoint(). */
+        bool Contains(const Vector3 &point) const;
+        bool Contains(const LineSegment &lineSegment) const;
+        bool Contains(const Triangle &triangle) const;
+        bool Contains(const Polygon &polygon) const;
+        bool Contains(const AABB &aabb) const;
+        bool Contains(const OBB &obb) const;
+        bool Contains(const Frustum &frustum) const;
+        bool Contains(const Polyhedron &polyhedron) const;
+
+        /// Computes the distance between this Frustum and the given object.
+        /** This function finds the nearest pair of points on this and the given object, and computes their distance.
+            If the two objects intersect, or one object is contained inside the other, the returned distance is zero.
+            @todo Add Frustum::Distance(Line/Ray/LineSegment/Plane/Triangle/Polygon/Circle/Disc/AABB/OBB/Capsule/Frustum/Polyhedron).
+            @see Contains(), Intersects(), ClosestPoint(). */
+        float Distance(const Vector3 &point) const;
+
+        /// Tests whether this Frustum and the given object intersect.
+        /** Both objects are treated as "solid", meaning that if one of the objects is fully contained inside
+            another, this function still returns true. (e.g. in case a line segment is contained inside this Frustum,
+            or this Frustum is contained inside a Sphere, etc.)
+            The first parameter of this function specifies the other object to test against.
+            @see Contains(), Distance(), ClosestPoint().
+            @todo Add Intersects(Circle/Disc). */
         bool Intersects(const Ray& ray) const;
         //bool Intersects(const Line& line) const;
         bool Intersects(const LineSegment& lineSegment) const;
@@ -229,7 +374,13 @@ namespace J3ML::Geometry
         bool Intersects(const Capsule& obb) const;
         bool Intersects(const Frustum& plane) const;
         bool Intersects(const Polyhedron& triangle) const;
-
+        /// Projects this Frustum onto the given 1D axis direction vector.
+        /** This function collapses this Frustum onto an 1D axis for the purposes of e.g. separate axis test computations.
+            The function returns a 1D range [outMin, outMax] denoting the interval of the projection.
+            @param direction The 1D axis to project to. This vector may be unnormalized, in which case the output
+                of this function gets scaled by the length of this vector.
+            @param outMin [out] Returns the minimum extent of this object along the projection axis.
+            @param outMax [out] Returns the maximum extent of this object along the projection axis. */
         void ProjectToAxis(const Vector3 &direction, float &outMin, float &outMax) const;
 
         void GetCornerPoints(Vector3 *outPointArray) const;
@@ -240,4 +391,8 @@ namespace J3ML::Geometry
 
         bool Intersects(const Line &line) const;
     };
+
+    Frustum operator * (const Matrix3x3& transform,  const Frustum& frustum);
+    Frustum operator * (const Matrix4x4& transform,  const Frustum& frustum);
+    Frustum operator * (const Quaternion& transform, const Frustum& frustum);
 }

include/J3ML/Geometry/KDTree.h

@@ -0,0 +1,12 @@
+#pragma once
+
+
+namespace J3ML::Geometry
+{
+
+    /// A KD-tree accelleration structure for static geometry.
+    class KdTree
+    {
+
+    };
+}

include/J3ML/Geometry/Triangle.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "J3ML/LinearAlgebra/Vector3.h"
 #include <J3ML/Geometry/Common.h>
 #include <J3ML/LinearAlgebra.h>
 #include <cfloat>
@@ -13,11 +14,18 @@ namespace J3ML::Geometry
         Vector3 V1;
         Vector3 V2;
     public:
-
         float DistanceSq(const Vector3 &point) const;
 
+        /// Returns a new triangle, translated with a direction vector
+        Triangle Translated(const Vector3& translation) const;
+        /// Returns a new triangle, scaled from 3D factors
+        Triangle Scaled(const Vector3& scaled) const;
+
         bool Intersects(const AABB& aabb) const;
         bool Intersects(const Capsule& capsule) const;
+        bool Intersects(const Triangle& rhs) const;
+        friend bool Intersects(const Triangle& lhs, const Triangle &rhs);
+
         AABB BoundingAABB() const;
 
         /// Tests if the given object is fully contained inside this triangle.
@@ -29,6 +37,8 @@ namespace J3ML::Geometry
         bool Contains(const LineSegment& lineSeg, float triangleThickness = 1e-3f) const;
         bool Contains(const Triangle& triangle, float triangleThickness = 1e-3f) const;
 
+        /// Project the triangle onto an axis, and returns the min and max value with the axis as a unit
+        Interval ProjectionInterval(const Vector3& axis) const;
         void ProjectToAxis(const Vector3 &axis, float &dMin, float &dMax) const;
 
         /// Quickly returns an arbitrary point inside this Triangle. Used in GJK intersection test.
@@ -108,6 +118,8 @@ namespace J3ML::Geometry
 
         Plane PlaneCW() const;
 
+        Vector3 FaceNormal() const;
+
         Vector3 Vertex(int i) const;
 
         LineSegment Edge(int i) const;

include/J3ML/J3ML.h

@@ -44,6 +44,8 @@ namespace J3ML::Math
 
     bool EqualAbs(float a, float b, float epsilon = 1e-3f);
     float RecipFast(float x);
+
+
     // Coming soon: Units Namespace
     // For Dimensional Analysis
     /*

include/J3ML/LinearAlgebra/AxisAngle.h

@@ -11,10 +11,13 @@ namespace J3ML::LinearAlgebra
     /// Transitional datatype, not useful for internal representation of rotation
     /// But has uses for conversion and manipulation.
     class AxisAngle {
+    public:
         Vector3 axis;
         float angle;
     public:
         AxisAngle();
+        explicit AxisAngle(const Quaternion& q);
+        explicit AxisAngle(const EulerAngle& e);
 
         AxisAngle(const Vector3 &axis, float angle);
 

include/J3ML/LinearAlgebra/EulerAngle.h

@@ -19,8 +19,8 @@ public:
     AxisAngle ToAxisAngle() const;
 
 
-    explicit EulerAngle(const Quaternion& orientation);
-    explicit EulerAngle(const AxisAngle& orientation);
+    explicit EulerAngle(const Quaternion& rhs);
+    explicit EulerAngle(const AxisAngle& rhs);
 
     /// TODO: Implement separate upper and lower bounds
     /// Preserves internal value of euler angles, normalizes and clamps the output.

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -11,11 +11,10 @@
 
 namespace J3ML::LinearAlgebra {
 
-    /// A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
-    /* This matrix can represent the most generic form of transformations for 3D objects,
-     * including perspective projections, which a 4-by-3 cannot store,
-     * and translations, which a 3-by-3 cannot represent.
-     * The elements of this matrix are
+    /// @brief A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
+    /// This matrix can represent the most generic form of transformations for 3D objects,
+    /// including perspective projections, which a 4-by-3 cannot store, and translations, which a 3-by-3 cannot represent.
+    /* 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, m_23,
@@ -227,9 +226,19 @@ namespace J3ML::LinearAlgebra {
         static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
         static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
 
+        /// Computes a left-handled orthographic projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
         static Matrix4x4 OpenGLOrthoProjLH(float n, float f, float h, float v);
+        /// Computes a right-handled orthographic projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
         static Matrix4x4 OpenGLOrthoProjRH(float n, float f, float h, float v);
+
+        /// Computes a left-handed perspective projection matrix for OpenGL.
+        /// @note Use the M*v multiplication order to project points with this matrix.
         static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
+        /// Identical to http://www.opengl.org/sdk/docs/man/xhtml/gluPerspective.xml , except uses viewport sizes instead of FOV to set up the
+        /// projection matrix.
+        /// @note Use the M*v multiplication order to project points with this matrix.
         static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);
 
         Vector4 operator[](int row);

include/J3ML/LinearAlgebra/Quaternion.h

@@ -38,10 +38,13 @@ namespace J3ML::LinearAlgebra
         //void Inverse();
 
         explicit Quaternion(Vector4 vector4);
+        explicit Quaternion(const EulerAngle& angle);
+        explicit Quaternion(const AxisAngle& angle);
 
         void SetFromAxisAngle(const Vector3 &vector3, float between);
 
         void SetFromAxisAngle(const Vector4 &vector4, float between);
+        void SetFrom(const AxisAngle& angle);
 
         Quaternion Inverse() const;
 
@@ -58,6 +61,8 @@ namespace J3ML::LinearAlgebra
 
         float GetAngle() const;
 
+        EulerAngle ToEulerAngle() const;
+
 
         Matrix3x3 ToMatrix3x3() const;
 

include/J3ML/LinearAlgebra/Vector3.h

@@ -8,11 +8,9 @@
 
 namespace J3ML::LinearAlgebra {
 
-// A 3D (x, y, z) ordered pair.
+/// A 3D (x, y, z) ordered pair.
 class Vector3 {
 public:
-
-
     float x = 0;
     float y = 0;
     float z = 0;

src/J3ML/Geometry/AABB.cpp

@@ -197,12 +197,12 @@ namespace J3ML::Geometry {
         return aabb;
     }
 
-    float AABB::GetVolume() const {
+    float AABB::Volume() const {
         Vector3 sz = Size();
         return sz.x * sz.y * sz.z;
     }
 
-    float AABB::GetSurfaceArea() const {
+    float AABB::SurfaceArea() const {
         Vector3 size = Size();
         return 2.f * (size.x*size.y + size.x*size.z + size.y*size.z);
     }
@@ -454,6 +454,51 @@ namespace J3ML::Geometry {
 #endif
     }
 
+    bool AABB::Intersects(const AABB& aabb) const {
+        return Intersection(aabb).has_value();
+    }
+
+    std::optional<AABB> AABB::Intersection(const AABB& rhs) const {
+        // Here we do SAT, except that due to both objects being AABBs, they are "already projected" onto the same axis
+        constexpr auto test = [](float a, float b, float c, float d) -> std::optional<Vector2> {
+            // Overlap Test
+            // Points go:
+            //       +-------------+
+            // +-----|-----+       |
+            // |  1  |     |     2 |
+            // |     +-----|-------+
+            // +-----------+
+            //
+            // A-----C-----B-------D
+            //
+            // IF A < C AND B > C ( Overlap in order object 1 -> object 2)
+            // IF C < A AND D > A ( Overlap in order object 2 -> object 1)
+            if (a < c && b > c) {
+                return Vector2{c, b};
+            }
+            if (c < a && d > a) {
+                return Vector2{a, d};
+            }
+            return std::nullopt;
+        };
+
+        // This is SAT, so we need all axes to collide
+        std::optional<Vector2> xCollision = test(MinX(), MaxX(), rhs.MinX(), rhs.MaxX());
+        if (!xCollision.has_value()) return std::nullopt;
+
+        std::optional<Vector2> yCollision = test(MinY(), MaxY(), rhs.MinY(), rhs.MaxY());
+        if (!yCollision.has_value()) return std::nullopt;
+
+        std::optional<Vector2> zCollision = test(MinZ(), MaxZ(), rhs.MinZ(), rhs.MaxZ());
+        if (!zCollision.has_value()) return std::nullopt;
+
+        // At this point all 3 optionals have a value ; x of each is the "min" value, y of each is the "max" value
+        return AABB{
+            Vector3{xCollision->x, yCollision->x, zCollision->x},
+            Vector3{xCollision->y, yCollision->y, zCollision->y}
+        };
+    }
+
     bool AABB::IntersectLineAABB_CPP(const Vector3 &linePos, const Vector3 &lineDir, float &tNear, float &tFar) const
     {
         assert(lineDir.IsNormalized());
@@ -552,6 +597,22 @@ namespace J3ML::Geometry {
         return AABB(minPoint+offset, maxPoint+offset);
     }
 
+    void AABB::Scale(const Vector3 &scale) {
+        minPoint.x *= scale.x;
+        minPoint.y *= scale.y;
+        minPoint.z *= scale.z;
+        maxPoint.x *= scale.x;
+        maxPoint.y *= scale.y;
+        maxPoint.z *= scale.z;
+    }
+
+    AABB AABB::Scaled(const Vector3 &scale) const {
+        return AABB(
+            Vector3(minPoint.x*scale.y, minPoint.y*scale.y, minPoint.z*scale.z),
+            Vector3(maxPoint.x*scale.y, maxPoint.y*scale.y, maxPoint.z*scale.z)
+        );
+    }
+
     AABB AABB::TransformAABB(const Matrix3x3 &transform) {
         // TODO: assert(transform.IsColOrthogonal());
         // TODO: assert(transform.HasUniformScale());

src/J3ML/Geometry/Common.cpp

@@ -0,0 +1,10 @@
+#include <J3ML/Geometry/Common.h>
+
+namespace J3ML::Geometry {
+
+    bool Interval::Intersects(const Interval& rhs) const {
+        return *this == rhs || this->min > rhs.max != this->max >= rhs.min;
+    }
+
+}
+

src/J3ML/Geometry/Frustum.cpp

@@ -293,4 +293,25 @@ namespace J3ML::Geometry
     {
         return this->ToPolyhedron().Intersects(polyhedron);
     }
+
+    Frustum::Frustum()
+            : type(FrustumType::Invalid),
+              pos(Vector3::NaN),
+              front(Vector3::NaN),
+              up(Vector3::NaN),
+              nearPlaneDistance(NAN),
+              farPlaneDistance(NAN),
+              worldMatrix(Matrix4x4::NaN),
+              viewProjectionMatrix(Matrix4x4::NaN)
+    {
+        // For conveniency, allow automatic initialization of the graphics API and handedness in use.
+        // If neither of the #defines are set, user must specify per-instance.
+    }
+
+    Vector3 Frustum::WorldRight() const {
+        if (handedness == FrustumHandedness::Right)
+            return Vector3::Cross(front, up);
+        else
+            return Vector3::Cross(up, front);
+    }
 }

src/J3ML/Geometry/Triangle.cpp

@@ -5,9 +5,42 @@
 #include <J3ML/Geometry/Line.h>
 #include <J3ML/Geometry/Capsule.h>
 
-
 namespace J3ML::Geometry
 {
+    Interval Triangle::ProjectionInterval(const Vector3& axis) const {
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        float min = axis.Dot(V0);
+        float max = min;
+
+        float value = axis.Dot(V1);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+
+        value = axis.Dot(V2);
+        if (value < min)
+            min = value;
+        if (value > max)
+            max = value;
+        return Interval{min, max};
+    }
+
+    Triangle Triangle::Translated(const Vector3& translation) const {
+        return {
+            V0 + translation,
+            V1 + translation,
+            V2 + translation
+        };
+    }
+
+    Triangle Triangle::Scaled(const Vector3& scale) const {
+        return {
+            {V0.x * scale.x, V0.y * scale.y, V0.z * scale.y},
+            {V1.x * scale.x, V1.y * scale.y, V1.z * scale.y},
+            {V2.x * scale.x, V2.y * scale.y, V2.z * scale.y},
+        };
+    }
 
     LineSegment Triangle::Edge(int i) const
     {
@@ -37,6 +70,13 @@ namespace J3ML::Geometry
             return Vector3::NaN;
     }
 
+    Vector3 Triangle::FaceNormal() const {
+        Vector3 edge1 = V1 - V0;
+        Vector3 edge2 = V2 - V1;
+
+        return edge1.Cross(edge2);
+    }
+
     Plane Triangle::PlaneCCW() const
     {
         return Plane(V0, V1, V2);
@@ -351,6 +391,78 @@ namespace J3ML::Geometry
         return capsule.Intersects(*this);
     }
 
+    namespace {
+        bool HaveSeparatingAxis(const Triangle& t1, const Triangle& t2, const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) {
+            // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/robust_sat.html
+            Vector3 ab = (a - b);
+            Vector3 axis = ab.Cross(c - d);
+
+            if (axis.IsZero()) {
+                // Axis is zero, they are parallel, try to find the vector orthogonal to both
+                Vector3 n = ab.Cross(c - a);
+
+                if (n.IsZero()) {
+                    // AB and AC are parallel, this means they are both on the same axis, just pick one
+                    axis = ab;
+                } else {
+                    // Parallel but not on the same axis, get the vector that is normal to both edges
+                    axis = ab.Cross(n);
+                }
+            }
+
+            return !t1.ProjectionInterval(axis).Intersects(t2.ProjectionInterval(axis));
+        }
+    }
+
+    bool Intersects(const Triangle& lhs, const Triangle& rhs) {
+        // Triangle v Triangle intersection check using SAT
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/generic_sat.html
+        // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter4/triangle-triangle.html
+        // Reminder, we only need to find *one* axis to disprove that they collide, the corrolary is that we need to check ALL axes to prove that they collide
+        
+        Vector3 lEdges[3] = {
+            lhs.V1 - lhs.V0,
+            lhs.V2 - lhs.V1,
+            lhs.V0 - lhs.V2,
+        };
+
+        // First, use lhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, lhs.V2, lhs.V1)) {
+            return false;
+        }
+
+        Vector3 rEdges[3] = {
+            rhs.V1 - rhs.V0,
+            rhs.V2 - rhs.V1,
+            rhs.V0 - rhs.V2,
+        };
+
+        // Second, use rhs's face normal as the separating axis
+        if (HaveSeparatingAxis(lhs, rhs, rhs.V1, rhs.V0, rhs.V2, rhs.V1)) {
+            return false;
+        }
+
+        // Third, check the normals of each edge against each other
+        if (HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V1, lhs.V0, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V2, lhs.V1, rhs.V0, rhs.V2) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V1, rhs.V0) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V2, rhs.V1) ||
+            HaveSeparatingAxis(lhs, rhs, lhs.V0, lhs.V2, rhs.V0, rhs.V2)) {
+            return false;
+        }
+
+        // No axis is separating, we can safely conclude they intersect
+        return true;
+    }
+
+    bool Triangle::Intersects(const Triangle& rhs) const {
+        return Geometry::Intersects(*this, rhs);
+    }
+
     Vector3 Triangle::ExtremePoint(const Vector3 &direction) const {
         Vector3 mostExtreme = Vector3::NaN;
         float mostExtremeDist = -FLT_MAX;

src/J3ML/LinearAlgebra/AxisAngle.cpp

@@ -14,4 +14,48 @@ namespace J3ML::LinearAlgebra {
                 std::cos(angle/2)
         };
     }
+
+    AxisAngle::AxisAngle(const Quaternion &q) {
+        auto theta = std::acos(q.w) * 2.f;
+        auto ax = q.x / std::sin(std::acos(theta));
+        auto ay = q.y / std::sin(std::acos(theta));
+        auto az = q.z / std::sin(std::acos(theta));
+    }
+
+    AxisAngle::AxisAngle(const EulerAngle &e) {
+
+        // Assuming the angles are in radians
+
+        float heading = e.pitch;
+        float attitude = e.yaw;
+        float bank = e.roll;
+
+        float c1 = std::cos(heading / 2.f);
+        float s1 = std::sin(heading / 2.f);
+        float c2 = std::cos(attitude / 2.f);
+        float s2 = std::sin(attitude / 2.f);
+        float c3 = std::cos(bank / 2.f);
+        float s3 = std::sin(bank / 2.f);
+
+        float w = c1*c2*c3 - s1*s2*s3;
+        float x = c1*c2*c3 + s1*s2*s3;
+        float y = s1*c2*c3 + c1*s2*s3;
+        float z = c1*s2*c3 - s1*c2*s3;
+
+        angle = 2.f * std::acos(w);
+
+        double norm = x*x + y*y + z*z;
+        if (norm < 0.001) { // when all euler angles are zero angle=0, so
+            // we can set axis to anything to avoid divide by zero
+            x = 1;
+            y = z = 0;
+        } else {
+            norm = std::sqrt(norm);
+            x /= norm;
+            y /= norm;
+            z /= norm;
+        }
+
+        axis = {x, y, z};
+    }
 }

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -48,4 +48,64 @@ namespace J3ML::LinearAlgebra {
     }
 
     EulerAngle::EulerAngle() : pitch(0), yaw(0), roll(0) {}
+
+    EulerAngle::EulerAngle(const AxisAngle &rhs) {
+
+        float x = rhs.axis.x;
+        float y = rhs.axis.y;
+        float z = rhs.axis.z;
+        float angle = rhs.angle;
+
+        double s = std::sin(rhs.angle);
+
+        double c = std::cos(rhs.angle);
+
+        double t = 1-c;
+
+        // if axis is not already normalized then uncomment this
+
+        // double magnitude = std::sqrt(x*x + y*y + z*z);
+        // if (magnitude == 0) throw error;
+        // x /= magnitude;
+        // y /= magnitude;
+        // z /= magnitude;
+
+        if ((x*y*t + z*s) > 0.998) { // North pole singularity detected
+            pitch = 2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if ((x*y*t + z*s) < -0.998) { // South pole singularity detected
+            pitch = -2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = -M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        pitch = std::atan2(y * s-x * z * t, 1 - (y*y + z*z) * t);
+        yaw = std::asin(x * y * t + z * s);
+        roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
+    }
+
+    EulerAngle::EulerAngle(const Quaternion &rhs) {
+        double test = rhs.x * rhs.y + rhs.z * rhs.w;
+        if (test > 0.499) { // Singularity at north pole
+            pitch = 2 * std::atan2(rhs.x, rhs.w);
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if (test < -0.499) { // Singularity at south pole
+            pitch = -2 * std::atan2(rhs.x, rhs.y);
+            yaw = - M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+        float sqx = rhs.x * rhs.x;
+        float sqy = rhs.y * rhs.y;
+        float sqz = rhs.z * rhs.z;
+    }
 }

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -201,4 +201,39 @@ namespace J3ML::LinearAlgebra {
     Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
         SetFromAxisAngle(rotationAxis, rotationAngleBetween);
     }
+
+    Quaternion::Quaternion(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
+
+    Quaternion::Quaternion(const EulerAngle &angle) {
+        // Abbreviations for the various angular functions
+        double cr = std::cos(angle.roll * 0.5);
+        double sr = std::sin(angle.roll * 0.5);
+        double cp = std::cos(angle.pitch * 0.5);
+        double sp = std::sin(angle.pitch * 0.5);
+        double cy = std::cos(angle.yaw * 0.5);
+        double sy = std::sin(angle.yaw * 0.5);
+
+        w = cr * cp * cy + sr * sp * sy;
+        x = sr * cp * cy - cr * sp * sy;
+        y = cr * sp * cy + sr * cp * sy;
+        z = cr * cp * sy - sr * sp * cy;
+    }
+
+    void Quaternion::SetFrom(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
+
+    EulerAngle Quaternion::ToEulerAngle() const {
+        return EulerAngle(*this);
+    }
 }

src/J3ML/LinearAlgebra/Vector2.cpp

@@ -86,10 +86,7 @@ namespace J3ML::LinearAlgebra {
 
     float Vector2::Dot(const Vector2& rhs) const
     {
-        auto a = this->Normalize();
-        auto b = rhs.Normalize();
-
-        return a.x * b.x + a.y * b.y;
+        return this->x * rhs.x + this->y * rhs.y;
     }
 
     Vector2 Vector2::Project(const Vector2& rhs) const

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -154,11 +154,9 @@ namespace J3ML::LinearAlgebra {
 
     float Vector3::Dot(const Vector3& rhs) const
     {
-        auto a = this->Normalize();
-        auto b = rhs.Normalize();
-        return a.x * b.x +
-               a.y * b.y +
-               a.z * b.z;
+        return x * rhs.x +
+               y * rhs.y +
+               z * rhs.z;
     }
 
     Vector3 Vector3::Project(const Vector3& rhs) const

tests/Geometry/Geometry.cpp

@@ -0,0 +1,51 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Common.h>
+
+using J3ML::Geometry::Interval;
+
+TEST(CommonGeometry, Interval_Intersect) {
+    // <- a ->
+    //             <- b ->
+    EXPECT_EQ((Interval{0, 1}.Intersects({2, 3})), false);
+
+    //             <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({0, 1})), false);
+
+    // <- a ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({3, 5})), true);
+
+    //    <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 4}.Intersects({1, 3})), true);
+
+    // <- a ->
+    //      <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({3, 5})), true);
+
+    //      <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{3, 5}.Intersects({2, 3})), true);
+
+    // <- a ->
+    // <- b     ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 5})), true);\
+
+    // <- a ->
+    // <- b ->
+    EXPECT_EQ((Interval{2, 3}.Intersects({2, 3})), true);
+
+    // . a
+    // . b
+    EXPECT_EQ((Interval{2, 2}.Intersects({2, 2})), true);
+
+    // <-    a    ->
+    //    <- b ->
+    EXPECT_EQ((Interval{2, 5}.Intersects({3, 4})), true);
+
+    //    <- a ->
+    // <-    b    ->
+    EXPECT_EQ((Interval{3, 4}.Intersects({2, 5})), true);
+}
+

tests/Geometry/TriangleTests.cpp

@@ -0,0 +1,89 @@
+#include <gtest/gtest.h>
+#include <J3ML/Geometry/Triangle.h>
+
+using J3ML::Geometry::Interval;
+using J3ML::Geometry::Triangle;
+
+TEST(TriangleTests, FaceNormal)
+{
+    Triangle t{
+        Vector3{-1, -1, -1},
+        Vector3{0,   1,  0},
+        Vector3{1,  -1,  1}
+    };
+
+    EXPECT_EQ(t.FaceNormal(), (Vector3{4, 0, -4}));
+}
+
+TEST(TriangleTests, IntersectTriangle)
+{
+    Triangle xyTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {2.0f, 0.0f, 0.0f}
+    };
+
+    // Triangle collides with itself
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle), true);
+    // Translate 1 towards x -- should collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(1.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards x -- should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(2.0f, 0.0f, 0.0f))), true);
+    // Translate 2 towards negative x -- should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-2.0f, 0.0f, 0.0f))), true);
+    // Translate 3 towards x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(3.0f, 0.0f, 0.0f))), false);
+    // Translate 3 towards negative x -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(-3.0f, 0.0f, 0.0f))), false);
+    // Translate 1 towards z -- should not collide
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+    // Triangle collides with contained smaller triangle
+    EXPECT_EQ(Intersects(xyTriangle, xyTriangle.Scaled(Vector3(0.5f, 0.5f, 0.5f)).Translated(Vector3(0.25f, 0.25f, 0.0f))), true);
+
+    Triangle zxTriangle {
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 0.0f, 1.0f},
+        {0.0f, 0.0f, 2.0f}
+    };
+
+    // Should collide exactly on V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle), true);
+    // Should collide across xyTriangle's edge and  zxTriangle's face
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -1.0))), true);
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -2.0))), true);
+    // xyTriangle's face should be poked by zxTriangle's V0
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 0.0f))), true);
+    // xyTriangle's face should be cut by zxTriangle
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, -0.5f))), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(1.0f, 1.0f, 1.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zxTriangle.Translated(Vector3(0.0f, 0.0f, -3.0f))), false);
+
+    Triangle yxTriangle{
+        {0.0f, 0.0f, 0.0f},
+        {1.0f, 1.0f, 0.0f},
+        {0.0f, 2.0f, 0.0f}
+    };
+
+    // Should collide on V0-V1 edge
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, yxTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(yxTriangle, yxTriangle.Translated(Vector3(0.0f, 0.0f, 1.0f))), false);
+
+    Triangle zyInvertedTriangle{
+        {0.0f, 1.0f, -1.0f},
+        {0.0f, 0.0f, 0.0f},
+        {0.0f, 1.0f, 1.0f}
+    };
+    // Should collide exactly on V1
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle), true);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.0f, 1.0f, 0.0f))), false);
+    // Should not collide
+    EXPECT_EQ(Intersects(xyTriangle, zyInvertedTriangle.Translated(Vector3(0.25f, 0.75f, 0.0f))), false);
+}
+

tests/LinearAlgebra/Vector2Tests.cpp

@@ -139,15 +139,15 @@ TEST(Vector2Test, V2_DotProduct)
     // TODO: Equality
     Vector2 A {2, 2};
     Vector2 B {1, 1};
-    EXPECT_FLOAT_EQ(A.Dot(B), 1.f);
+    EXPECT_FLOAT_EQ(A.Dot(B), 4.f);
 }
 
 TEST(Vector2Test, V2_Project)
 {
-    Vector2 Base {1, 1};
-    Vector2 Projected {1, 1};
+    Vector2 Base {4, 4};
+    Vector2 Projected {1, 0};
 
-    Vector2 ExpectedResult {0.5, 0.5};
+    Vector2 ExpectedResult {4, 0};
 
     EXPECT_EQ(Base.Project(Projected), ExpectedResult);
 }

tests/LinearAlgebra/Vector3Tests.cpp

@@ -152,7 +152,7 @@ TEST(Vector3Test, V3_DotProduct) {
     Vector3 B{1,1,1};
 
 
-    float ExpectedResult = 1;
+    float ExpectedResult = 18;
 
     EXPECT_FLOAT_EQ(A.Dot(B), ExpectedResult);
 }