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
auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0});
Simplex points;
points.push_front(support);
Simplex simplex;
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;
}
}

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

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