admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-02-13 07:03:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

updated

Browse Source
This commit is contained in:
Orange
2025-11-09 15:39:11 +03:00
parent cd18b088cb
commit 043b5c588d
2 changed files with 126 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
include/omath/collision/gjk_algorithm.hpp
View File

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

146
include/omath/collision/simplex.hpp
View File

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