updated

include/omath/collision/gjk_algorithm.hpp

@@ -25,8 +25,8 @@ namespace omath::collision
             // Get initial support point in any direction
             auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0});
 
-            Simplex points;
+            Simplex simplex;
-            points.push_front(support);
+            simplex.push_front(support);
 
             auto direction = -support;
 
@@ -37,9 +37,9 @@ namespace omath::collision
                 if (support.dot(direction) <= 0.f)
                     return false;
 
-                points.push_front(support);
+                simplex.push_front(support);
 
-                if (handle_simplex(points, direction))
+                if (simplex.handle(direction))
                     return true;
             }
         }

include/omath/collision/simplex.hpp

@@ -8,152 +8,168 @@
 
 namespace omath::collision
 {
+    template<class VectorType = Vector3<float>>
     class Simplex
     {
-        std::array<Vector3<float>, 4> m_points;
+        std::array<VectorType, 4> m_points;
-        int m_size;
+        std::size_t m_size;
 
     public:
-        Simplex(): m_size(0)
+        constexpr Simplex(): m_size(0)
         {
         }
 
-        Simplex& operator=(const std::initializer_list<Vector3<float>> list)
+        constexpr Simplex& operator=(const std::initializer_list<VectorType>& list)
         {
             m_size = 0;
 
-            for (const Vector3<float>& point : list)
+            for (const VectorType& point : list)
                 m_points[m_size++] = point;
 
             return *this;
         }
 
-        void push_front(const Vector3<float>& point)
+        constexpr void push_front(const VectorType& point)
         {
             m_points = {point, m_points[0], m_points[1], m_points[2]};
-            m_size = std::min(m_size + 1, 4);
+            m_size = std::min<std::size_t>(m_size + 1, 4);
         }
 
-        Vector3<float>& operator[](const std::size_t i)
+        constexpr const VectorType& operator[](const std::size_t i) const
         {
             return m_points[i];
         }
-        [[nodiscard]] std::size_t size() const
+        [[nodiscard]]
+        constexpr std::size_t size() const
         {
             return m_size;
         }
 
-        [[nodiscard]] auto begin() const
+        [[nodiscard]]
+        constexpr auto begin() const
         {
             return m_points.begin();
         }
-        [[nodiscard]] auto end() const
+        [[nodiscard]]
+        constexpr auto end() const
         {
             return m_points.end() - (4 - m_size);
         }
-    };
+        [[nodiscard]]
-
+        constexpr bool handle(VectorType& direction)
-    bool handle_line(Simplex& points, Vector3<float>& direction)
-    {
-        const Vector3<float>& a = points[0];
-        const Vector3<float>& b = points[1];
-
-        const Vector3<float> ab = b - a;
-        // ReSharper disable once CppTooWideScopeInitStatement
-        const Vector3<float> ao = -a;
-
-        if (ab.point_to_same_direction(ao))
-            direction = ab.cross(ao).cross(ab);
-        else
         {
-            points = {a};
+            switch (m_points.size())
-            direction = ao;
-        }
-
-        return false;
-    }
-
-    bool handle_triangle(Simplex& points, Vector3<float>& direction)
-    {
-        const Vector3<float>& a = points[0];
-        const Vector3<float>& b = points[1];
-        const Vector3<float>& c = points[2];
-
-        const Vector3<float> ab = b - a;
-        const Vector3<float> ac = c - a;
-        const Vector3<float> ao = -a;
-
-        const Vector3<float> abc = ab.cross(ac);
-
-        if (abc.cross(ac).point_to_same_direction(ao))
-        {
-            if (ac.point_to_same_direction(ao))
             {
-                points = {a, c};
+            case 2:
-                direction = ac.cross(ao).cross(ac);
+                return handle_line(direction);
-
+            case 3:
-                return false;
+                return handle_triangle(direction);
+            case 4:
+                return handle_tetrahedron(direction);
+            default:
+                std::unreachable();
             }
-            return handle_line(points = {a, b}, direction);
         }
-
+    private:
-        if (ab.cross(abc).point_to_same_direction(ao))
+        [[nodiscard]]
-            return handle_line(points = {a, b}, direction);
+        constexpr bool handle_line(VectorType& direction)
-
-        if (abc.point_to_same_direction(ao))
         {
-            direction = abc;
+            const auto& a = m_points[0];
-        }
+            const auto& b = m_points[1];
-        else
-        {
-            points = {a, c, b};
-            direction = -abc;
-        }
 
-        return false;
+            const auto ab = b - a;
-    }
+            // ReSharper disable once CppTooWideScopeInitStatement
+            const auto ao = -a;
 
-    bool handle_tetrahedron(Simplex& simplex, Vector3<float>& direction)
+            if (ab.point_to_same_direction(ao))
-    {
+                direction = ab.cross(ao).cross(ab);
-        const auto& a = simplex[0];
+            else
-        const auto& b = simplex[1];
+            {
-        const auto& c = simplex[2];
+                *this = {a};
-        const auto& d = simplex[3];
+                direction = ao;
+            }
 
-        const Vector3<float> ab = b - a;
-        const Vector3<float> ac = c - a;
-        const Vector3<float> ad = d - a;
-        const Vector3<float> ao = -a;
-
-        const Vector3<float> abc = ab.cross(ac);
-        const Vector3<float> acd = ac.cross(ad);
-        const Vector3<float> adb = ad.cross(ab);
-
-        if (abc.point_to_same_direction(ao))
-            return handle_triangle(simplex = {a, b, c}, direction);
-
-        if (acd.point_to_same_direction(ao))
-            return handle_triangle(simplex = {a, c, d}, direction);
-
-        if (adb.point_to_same_direction(ao))
-            return handle_triangle(simplex = {a, d, b}, direction);
-
-        return true;
-    }
-
-    [[nodiscard]]
-    bool handle_simplex(Simplex& points, Vector3<float>& direction)
-    {
-        switch (points.size())
-        {
-        case 2:
-            return handle_line(points, direction);
-        case 3:
-            return handle_triangle(points, direction);
-        case 4:
-            return handle_tetrahedron(points, direction);
-        default:
             return false;
         }
-    }
+        [[nodiscard]]
+        constexpr bool handle_triangle(VectorType& direction)
+        {
+            const auto& a = m_points[0];
+            const auto& b = m_points[1];
+            const auto& c = m_points[2];
+
+            const auto ab = b - a;
+            const auto ac = c - a;
+            const auto ao = -a;
+
+            const auto abc = ab.cross(ac);
+
+            if (abc.cross(ac).point_to_same_direction(ao))
+            {
+                if (ac.point_to_same_direction(ao))
+                {
+                    *this = {a, c};
+                    direction = ac.cross(ao).cross(ac);
+
+                    return false;
+                }
+                *this = {a, b};
+                return handle_line(direction);
+            }
+
+            if (ab.cross(abc).point_to_same_direction(ao))
+            {
+                *this = {a, b};
+                return handle_line(direction);
+            }
+            if (abc.point_to_same_direction(ao))
+            {
+                direction = abc;
+            }
+            else
+            {
+                *this = {a, c, b};
+                direction = -abc;
+            }
+
+            return false;
+        }
+        [[nodiscard]]
+        constexpr bool handle_tetrahedron(VectorType& direction)
+        {
+            const auto& a = m_points[0];
+            const auto& b = m_points[1];
+            const auto& c = m_points[2];
+            const auto& d = m_points[3];
+
+            const auto ab = b - a;
+            const auto ac = c - a;
+            const auto ad = d - a;
+            const auto ao = -a;
+
+            const auto abc = ab.cross(ac);
+            const auto acd = ac.cross(ad);
+            const auto adb = ad.cross(ab);
+
+            if (abc.point_to_same_direction(ao))
+            {
+                *this = {a, b, c};
+                return handle_triangle(direction);
+            }
+
+            if (acd.point_to_same_direction(ao))
+            {
+                *this = {a, c, d};
+                return handle_triangle(direction);
+            }
+
+            if (adb.point_to_same_direction(ao))
+            {
+                *this = {a, d, b};
+                return handle_triangle(direction);
+            }
+            return true;
+        }
+    };
+
 } // namespace omath::collision