admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-02-12 22:53:27 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #105 from orange-cpp/feature/epa_improvement

Browse Source 
Feature/epa improvement
This commit is contained in:
Orange++
2025-11-13 22:13:26 +03:00
committed by GitHub
parent a5b24f90dc 0283935918
commit dd8c41b19f
3 changed files with 40 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
include/omath/collision/epa_algorithm.hpp
View File

@@ -29,8 +29,8 @@ namespace omath::collision
struct Result final
{
bool success{false};
Vertex normal{}; // outward normal (from B to A)
Vertex penetration_vector;
float depth{0.0f};
int iterations{0};
int num_vertices{0};
@@ -45,8 +45,8 @@ namespace omath::collision
// Precondition: simplex.size()==4 and contains the origin.
[[nodiscard]]
static Result solve(const ColliderType& a, const ColliderType& b, const Simplex<Vertex>& simplex,
const Params params = {})
static std::optional<Result> solve(const ColliderType& a, const ColliderType& b, const Simplex<Vertex>& simplex,
const Params params = {})
{
// --- Build initial polytope from simplex (4 points) ---
std::vector<Vertex> vertexes;
@@ -90,12 +90,16 @@ namespace omath::collision
// Converged if we can’t push the face closer than tolerance
if (p_dist - f.d <= params.tolerance)
{
out.success = true;
out.normal = f.n;
out.depth = f.d; // along unit normal
out.iterations = it + 1;
out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size());
const auto centers = b.get_origin() - a.get_origin();
const auto sign = out.normal.dot(centers) >= 0 ? 1 : -1;
out.penetration_vector = out.normal * out.depth * sign;
return out;
}
@@ -149,13 +153,19 @@ namespace omath::collision
for (const auto& f : faces)
if (f.d < best.d)
best = f;
out.success = true;
out.normal = best.n;
out.depth = best.d;
out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size());
const auto centers = b.get_origin() - a.get_origin();
const auto sign = out.normal.dot(centers) >= 0 ? 1 : -1;
out.penetration_vector = out.normal * out.depth * sign;
return out;
}
return out;
return std::nullopt;
}
private:

5
include/omath/collision/mesh_collider.hpp
View File

@@ -31,6 +31,11 @@ namespace omath::collision
return m_mesh.vertex_to_world_space(find_furthest_vertex(direction));
}
[[nodiscard]]
const VertexType& get_origin() const
{
return m_mesh.get_origin();
}
private:
MeshType m_mesh;
};

38
tests/general/unit_test_epa.cpp
View File

@@ -36,22 +36,22 @@ TEST(UnitTestEpa, TestCollisionTrue)
params.max_iterations = 64;
params.tolerance = 1e-4f;
auto epa = EPA::solve(A, B, gjk.simplex, params);
ASSERT_TRUE(epa.success) << "EPA should converge";
ASSERT_TRUE(epa.has_value()) << "EPA should converge";
// Normal is unit
EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f);
EXPECT_NEAR(epa->normal.dot(epa->normal), 1.0f, 1e-5f);
// For this setup, depth ≈ 1.5 (2 - 0.5)
EXPECT_NEAR(epa.depth, 1.5f, 1e-3f);
EXPECT_NEAR(epa->depth, 1.5f, 1e-3f);
// Normal axis sanity: near X axis
EXPECT_NEAR(std::abs(epa.normal.x), 1.0f, 1e-3f);
EXPECT_NEAR(epa.normal.y, 0.0f, 1e-3f);
EXPECT_NEAR(epa.normal.z, 0.0f, 1e-3f);
EXPECT_NEAR(std::abs(epa->normal.x), 1.0f, 1e-3f);
EXPECT_NEAR(epa->normal.y, 0.0f, 1e-3f);
EXPECT_NEAR(epa->normal.z, 0.0f, 1e-3f);
// Try both signs with a tiny margin (avoid grazing contacts)
const float margin = 1.0f + 1e-3f;
const auto pen = epa.normal * epa.depth;
const auto pen = epa->normal * epa->depth;
Mesh b_plus = b;
b_plus.set_origin(b_plus.get_origin() + pen * margin);
@@ -102,25 +102,25 @@ TEST(UnitTestEpa, TestCollisionTrue2)
params.max_iterations = 64;
params.tolerance = 1e-4f;
auto epa = EPA::solve(A, B, gjk.simplex, params);
ASSERT_TRUE(epa.success) << "EPA should converge";
ASSERT_TRUE(epa.has_value()) << "EPA should converge";
// Normal is unit-length
EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f);
EXPECT_NEAR(epa->normal.dot(epa->normal), 1.0f, 1e-5f);
// For centers at 0 and +0.5 and half-extent 1 -> depth ≈ 1.5
EXPECT_NEAR(epa.depth, 1.5f, 1e-3f);
EXPECT_NEAR(epa->depth, 1.5f, 1e-3f);
// Axis sanity: mostly X
EXPECT_NEAR(std::abs(epa.normal.x), 1.0f, 1e-3f);
EXPECT_NEAR(epa.normal.y, 0.0f, 1e-3f);
EXPECT_NEAR(epa.normal.z, 0.0f, 1e-3f);
EXPECT_NEAR(std::abs(epa->normal.x), 1.0f, 1e-3f);
EXPECT_NEAR(epa->normal.y, 0.0f, 1e-3f);
EXPECT_NEAR(epa->normal.z, 0.0f, 1e-3f);
// Choose a deterministic sign: orient penetration from A toward B
const auto centers = b.get_origin() - a.get_origin(); // (0.5, 0, 0)
float sign = (epa.normal.dot(centers) >= 0.0f) ? +1.0f : -1.0f;
float sign = (epa->normal.dot(centers) >= 0.0f) ? +1.0f : -1.0f;
constexpr float margin = 1.0f + 1e-3f; // tiny slack to avoid grazing
const auto pen = epa.normal * epa.depth * sign;
const auto pen = epa->normal * epa->depth * sign;
// Apply once: B + pen must separate; the opposite must still collide
Mesh b_resolved = b;
@@ -132,8 +132,8 @@ TEST(UnitTestEpa, TestCollisionTrue2)
EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))) << "Opposite direction should still intersect";
// Some book-keeping sanity
EXPECT_GT(epa.iterations, 0);
EXPECT_LT(epa.iterations, params.max_iterations);
EXPECT_GE(epa.num_faces, 4);
EXPECT_GT(epa.num_vertices, 4);
EXPECT_GT(epa->iterations, 0);
EXPECT_LT(epa->iterations, params.max_iterations);
EXPECT_GE(epa->num_faces, 4);
EXPECT_GT(epa->num_vertices, 4);
}