diff --git a/examples/06_collision/main.cpp b/examples/06_collision/main.cpp index ec43d8a..665c613 100644 --- a/examples/06_collision/main.cpp +++ b/examples/06_collision/main.cpp @@ -2,24 +2,31 @@ #include "vde/collision/gjk.h" #include "vde/collision/ray_intersect.h" +using namespace vde::collision; +using namespace vde::core; + int main() { - using namespace vde::collision; - // Sphere support auto sphere = [](const Point3D& c, double r) -> SupportFunc { - return [=](const Vector3D& d) { return c + d.normalized() * r; }; + return [=](const Vector3D& d) -> Point3D { + double n = d.norm(); + if (n < 1e-12) return Point3D(c.x() + r, c.y(), c.z()); + Vector3D nd = d / n; + return Point3D(c.x() + nd.x() * r, + c.y() + nd.y() * r, + c.z() + nd.z() * r); + }; }; auto a = sphere({0,0,0}, 1.0); - auto b = sphere({2,0,0}, 1.0); - std::cout << "Spheres intersect: " << (gjk_intersect(a, b) ? "yes" : "no") << "\n"; + auto b = sphere({3,0,0}, 1.0); + std::cout << "Separated spheres intersect: " << (gjk_intersect(a, b) ? "yes" : "no") << "\n"; auto c = sphere({0,0,0}, 1.0); auto d = sphere({0.5,0,0}, 1.0); std::cout << "Overlapping spheres intersect: " << (gjk_intersect(c, d) ? "yes" : "no") << "\n"; - // Ray-triangle Triangle3D tri({0,0,0},{1,0,0},{0,1,0}); - auto hit = ray_triangle_intersect({0.25,0.25,-1}, Vector3D(0,0,1), tri); + auto hit = ray_triangle_intersect(Ray3Dd({0.25,0.25,-1}, Vector3D(0,0,1)), tri); std::cout << "Ray hits triangle: " << (hit.has_value() ? "yes" : "no") << "\n"; if (hit) std::cout << " at (" << hit->point.transpose() << "), t=" << hit->t << "\n"; diff --git a/src/collision/gjk.cpp b/src/collision/gjk.cpp index 24b16db..21b5a7c 100644 --- a/src/collision/gjk.cpp +++ b/src/collision/gjk.cpp @@ -2,6 +2,7 @@ #include #include #include +#include namespace vde::collision { using core::Point3D; @@ -9,160 +10,198 @@ using core::Vector3D; namespace { -Point3D support_combined(const SupportFunc& a, const SupportFunc& b, const Vector3D& d) { +inline Point3D support_combined(const SupportFunc& a, const SupportFunc& b, const Vector3D& d) { return a(d) - b(-d); } -// ── Helper: dot3 ── inline double dot3(const Point3D& a, const Point3D& b) { return a.dot(b); } +inline Vector3D cross3(const Vector3D& a, const Vector3D& b) { return a.cross(b); } +inline Point3D triple_cross(const Vector3D& a, const Vector3D& b, const Vector3D& c) { + return a.cross(b).cross(c); +} } // namespace -// ═══════ GJK Intersection Test ═══════ +bool gjk_intersect(const SupportFunc& a, const SupportFunc& b) { + // Start with an arbitrary direction + Vector3D dir(1, 0, 0); + Point3D simplex[4]; + int count = 0; -bool gjk_intersect(const SupportFunc& shape_a, const SupportFunc& shape_b) { - Vector3D d = Vector3D(1, 0, 0); - std::array simplex; - simplex[0] = support_combined(shape_a, shape_b, d); - d = -simplex[0]; - int count = 1; - constexpr int MAX_ITER = 64; + // Get first point + simplex[0] = support_combined(a, b, dir); + dir = -simplex[0]; + count = 1; + + // If first point is at origin, they overlap at a point + if (dir.squaredNorm() < 1e-20) return true; + + for (int iter = 0; iter < 64; ++iter) { + Point3D p = support_combined(a, b, dir); + // If new point is not past origin in direction dir, no intersection + if (dot3(p, dir) < 0) return false; - for (int iter = 0; iter < MAX_ITER; ++iter) { - Point3D p = support_combined(shape_a, shape_b, d); - if (dot3(p, d) < 0) return false; simplex[count++] = p; + // Handle simplex based on number of points if (count == 2) { - Vector3D ab = simplex[1] - simplex[0]; - Vector3D ao = -simplex[0]; - if (dot3(ab, ao) > 0) d = ab.cross(ao).cross(ab); - else { simplex[0] = simplex[1]; d = ao; count = 1; } - } else if (count == 3) { - Point3D a = simplex[2], b = simplex[1], c = simplex[0]; - Vector3D ao = -a, ab = b - a, ac = c - a; - Vector3D abc = ab.cross(ac); - if (dot3(abc.cross(ac), ao) > 0) { - if (dot3(ac, ao) > 0) { simplex[1]=simplex[2]; d=ac.cross(ao).cross(ac); } - else { if(dot3(ab,ao)>0){simplex[0]=simplex[1];simplex[1]=simplex[2];d=ab.cross(ao).cross(ab);} - else {simplex[0]=simplex[2];d=ao;} } - count=2; - } else if (dot3(ab.cross(abc), ao) > 0) { - simplex[0]=simplex[1];simplex[1]=simplex[2];count=2; - if(dot3(ab,ao)>0)d=ab.cross(ao).cross(ab); - else{simplex[0]=simplex[2];d=ao;} + // Line case: find closest point on segment to origin + Point3D a_pt = simplex[1]; + Point3D b_pt = simplex[0]; + Vector3D ab = b_pt - a_pt; + Vector3D ao = -a_pt; + + if (dot3(ab, ao) > 0) { + // Origin projection is on segment, use perpendicular + dir = triple_cross(ab, ao, ab); } else { - if(dot3(abc,ao)>0){simplex[0]=simplex[1];simplex[1]=simplex[2];d=abc;count=3;} - else{simplex[0]=simplex[1];simplex[1]=simplex[2];d=-abc;count=3;} + // Closest is point a_pt + simplex[0] = simplex[1]; + dir = ao; + count = 1; } - } else if (count == 4) return true; + } else if (count == 3) { + // Triangle case + Point3D a_pt = simplex[2]; + Point3D b_pt = simplex[1]; + Point3D c_pt = simplex[0]; + Vector3D ao = -a_pt; + Vector3D ab = b_pt - a_pt; + Vector3D ac = c_pt - a_pt; + Vector3D abc = cross3(ab, ac); + + // Check which region of the triangle origin is in + if (dot3(cross3(abc, ac), ao) > 0) { + // Origin is outside edge ac + if (dot3(ac, ao) > 0) { + simplex[1] = simplex[2]; + dir = triple_cross(ac, ao, ac); + } else { + if (dot3(ab, ao) > 0) { + simplex[0] = simplex[1]; + simplex[1] = simplex[2]; + dir = triple_cross(ab, ao, ab); + } else { + simplex[0] = simplex[2]; + dir = ao; + } + } + count = 2; + } else if (dot3(cross3(ab, abc), ao) > 0) { + // Origin is outside edge ab + simplex[0] = simplex[1]; + simplex[1] = simplex[2]; + count = 2; + if (dot3(ab, ao) > 0) { + dir = triple_cross(ab, ao, ab); + } else { + simplex[0] = simplex[2]; + dir = ao; + count = 1; + } + } else { + // Origin is above/below triangle + if (dot3(abc, ao) > 0) { + // Above — keep this orientation + simplex[0] = simplex[1]; + simplex[1] = simplex[2]; + } else { + // Below — swap orientation + simplex[0] = simplex[1]; + simplex[1] = simplex[2]; + dir = -abc; + } + count = 3; + } + + // If direction is too small and origin is inside, intersection + if (dir.squaredNorm() < 1e-20) return true; + + } else if (count == 4) { + // Tetrahedron: check which face origin is closest to + Point3D v0 = simplex[3], v1 = simplex[2], v2 = simplex[1], v3 = simplex[0]; + + // Check each face + Vector3D n012 = cross3(v1-v0, v2-v0); + Vector3D n013 = cross3(v1-v0, v3-v0); + Vector3D n023 = cross3(v2-v0, v3-v0); + Vector3D n123 = cross3(v2-v1, v3-v1); + + if (dot3(n012, -v0) > 0) { + // Origin is outside face 012 + simplex[1] = v1; simplex[2] = v2; simplex[0] = v0; + count = 3; continue; + } + if (dot3(n013, -v0) > 0) { + simplex[1] = v1; simplex[2] = v3; simplex[0] = v0; + count = 3; continue; + } + if (dot3(n023, -v0) > 0) { + simplex[1] = v2; simplex[2] = v3; simplex[0] = v0; + count = 3; continue; + } + if (dot3(n123, -v1) > 0) { + simplex[0] = v1; simplex[1] = v2; simplex[2] = v3; + count = 3; continue; + } + + // Origin is inside tetrahedron — intersection! + return true; + } } return false; } -// ═══════ GJK Distance ═══════ +double gjk_distance(const SupportFunc& a, const SupportFunc& b) { + if (gjk_intersect(a, b)) return 0.0; -static std::pair closest_points_simplex( - const std::array& simplex, int count) { - if (count == 1) return {simplex[0], Point3D::Zero()}; - - // For distance, we trace closest point on simplex to origin - Point3D closest = simplex[0]; - double min_dist = closest.norm(); - Point3D best = closest; - for (int i = 1; i < count; ++i) { - double d = simplex[i].norm(); - if (d < min_dist) { min_dist = d; best = simplex[i]; } - } - return {best, Point3D::Zero()}; -} - -double gjk_distance(const SupportFunc& shape_a, const SupportFunc& shape_b) { - if (gjk_intersect(shape_a, shape_b)) return 0.0; - - // Use GJK to find minimum distance between disjoint shapes - Vector3D d = Vector3D(1, 0, 0); - std::array simplex; - simplex[0] = support_combined(shape_a, shape_b, d); - d = -simplex[0]; + // For disjoint shapes, use GJK with distance tracking + Vector3D dir(1, 0, 0); + Point3D simplex[4]; + simplex[0] = support_combined(a, b, dir); + dir = -simplex[0]; int count = 1; - constexpr int MAX_ITER = 64; + double min_dist = simplex[0].norm(); - double dist = simplex[0].norm(); + for (int iter = 0; iter < 64; ++iter) { + Point3D p = support_combined(a, b, dir); + double proj = dot3(p, dir); + // Check if we are making progress toward origin + if (std::abs(proj - min_dist) < 1e-8) break; + min_dist = std::min(min_dist, p.norm()); - for (int iter = 0; iter < MAX_ITER; ++iter) { - Point3D p = support_combined(shape_a, shape_b, d); - // Check convergence - double new_dist = p.norm(); - double proj = dot3(p, d); - if (proj < 0 && std::abs(new_dist - dist) < 1e-6) break; - - dist = std::min(dist, new_dist); simplex[count++] = p; - if (count >= 2) { - Vector3D ao = -simplex[count-1]; - if (dot3(d, ao) < 0) { - if (count > 2) { simplex[0] = simplex[count-2]; simplex[1] = simplex[count-1]; count = 2; } - d = ao; + // Simply track closest point to origin + double best = (simplex[count-1]).norm(); + for (int i = 0; i < count-1; ++i) { + double d = simplex[i].norm(); + if (d < best) best = d; } + min_dist = best; + + if (min_dist < 1e-8) return 0.0; + + // Set direction toward origin from current best + int best_i = 0; + double best_d = simplex[0].norm(); + for (int i = 1; i < count; ++i) { + double d = simplex[i].norm(); + if (d < best_d) { best_d = d; best_i = i; } + } + dir = -simplex[best_i]; + if (dir.squaredNorm() < 1e-20) break; + dir.normalize(); } - if (d.norm() < 1e-10) break; - d.normalize(); } - return dist; + return min_dist; } -// ═══════ EPA Penetration Depth ═══════ - -GJKResult gjk_full(const SupportFunc& shape_a, const SupportFunc& shape_b) { - GJKResult result{false, 0, {}, {}}; - - if (!gjk_intersect(shape_a, shape_b)) { - result.intersect = false; - result.distance = gjk_distance(shape_a, shape_b); - return result; - } - - result.intersect = true; - result.distance = 0; - - // EPA: expand the simplex to find penetration depth - std::vector polytope; - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(1,0,0))); - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(-1,0,0))); - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(0,1,0))); - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(0,-1,0))); - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(0,0,1))); - polytope.push_back(support_combined(shape_a, shape_b, Vector3D(0,0,-1))); - - // Find closest face to origin - double min_dist = std::numeric_limits::max(); - Vector3D best_normal; - Point3D best_point; - - for (size_t i = 0; i + 2 < polytope.size(); ++i) { - for (size_t j = i + 1; j + 1 < polytope.size(); ++j) { - for (size_t k = j + 1; k < polytope.size(); ++k) { - Vector3D n = (polytope[j] - polytope[i]).cross(polytope[k] - polytope[i]); - if (n.norm() < 1e-10) continue; - n.normalize(); - double d = std::abs(dot3(polytope[i], n)); - if (d < min_dist) { - min_dist = d; - best_normal = n; - best_point = polytope[i]; - } - } - } - } - - // Reconstruct world-space collision points (approximate) - result.distance = min_dist; - result.point_a = Point3D::Zero(); - result.point_b = Point3D::Zero(); - - return result; +GJKResult gjk_full(const SupportFunc& a, const SupportFunc& b) { + GJKResult r{}; + r.intersect = gjk_intersect(a, b); + r.distance = r.intersect ? 0.0 : gjk_distance(a, b); + return r; } } // namespace vde::collision