#include <gtest/gtest.h>
#include "vde/collision/gjk.h"
#include <cmath>

using namespace vde::collision;

static SupportFunc make_sphere(const Point3D& center, double radius) {
    return [=](const Vector3D& dir) -> Point3D {
        double n = dir.norm();
        if (n < 1e-12) return Point3D(center.x() + radius, center.y(), center.z());
        Vector3D nd = dir / n;
        return Point3D(center.x() + nd.x() * radius,
                       center.y() + nd.y() * radius,
                       center.z() + nd.z() * radius);
    };
}

TEST(GJKTest, SeparatedSpheres) {
    auto a = make_sphere({0,0,0}, 1.0);
    auto b = make_sphere({5,0,0}, 1.0);
    EXPECT_FALSE(gjk_intersect(a, b));
}

TEST(GJKTest, OverlappingSpheres) {
    auto a = make_sphere({0,0,0}, 1.0);
    auto b = make_sphere({1.5,0,0}, 1.0);
    EXPECT_TRUE(gjk_intersect(a, b));
}