mirror of
https://github.com/orange-cpp/omath.git
synced 2026-02-13 07:03:25 +00:00
updated
This commit is contained in:
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
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
|
||||
[[nodiscard]]
|
||||
constexpr bool handle(VectorType& direction)
|
||||
{
|
||||
points = {a};
|
||||
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))
|
||||
switch (m_points.size())
|
||||
{
|
||||
points = {a, c};
|
||||
direction = ac.cross(ao).cross(ac);
|
||||
|
||||
return false;
|
||||
case 2:
|
||||
return handle_line(direction);
|
||||
case 3:
|
||||
return handle_triangle(direction);
|
||||
case 4:
|
||||
return handle_tetrahedron(direction);
|
||||
default:
|
||||
std::unreachable();
|
||||
}
|
||||
return handle_line(points = {a, b}, direction);
|
||||
}
|
||||
|
||||
if (ab.cross(abc).point_to_same_direction(ao))
|
||||
return handle_line(points = {a, b}, direction);
|
||||
|
||||
if (abc.point_to_same_direction(ao))
|
||||
private:
|
||||
[[nodiscard]]
|
||||
constexpr bool handle_line(VectorType& direction)
|
||||
{
|
||||
direction = abc;
|
||||
}
|
||||
else
|
||||
{
|
||||
points = {a, c, b};
|
||||
direction = -abc;
|
||||
}
|
||||
const auto& a = m_points[0];
|
||||
const auto& b = m_points[1];
|
||||
|
||||
return false;
|
||||
}
|
||||
const auto ab = b - a;
|
||||
// ReSharper disable once CppTooWideScopeInitStatement
|
||||
const auto ao = -a;
|
||||
|
||||
bool handle_tetrahedron(Simplex& simplex, Vector3<float>& direction)
|
||||
{
|
||||
const auto& a = simplex[0];
|
||||
const auto& b = simplex[1];
|
||||
const auto& c = simplex[2];
|
||||
const auto& d = simplex[3];
|
||||
if (ab.point_to_same_direction(ao))
|
||||
direction = ab.cross(ao).cross(ab);
|
||||
else
|
||||
{
|
||||
*this = {a};
|
||||
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
|
||||
Reference in New Issue
Block a user