#include "math.h"
#include <cmath>
#include "gjk_interface.hpp"

#define UNUSED(x) (void)(x)
#define POLY_SIMPLIFY_MAX_SIZE 4
using namespace Eigen;

namespace gjk_interface {
    const Vector3d support(const std::vector<Vector3d>& poly, const Vector3d& dir)
    {
        float maxDot = -INFINITY;
        Vector3d supportPoint;

        for (const Vector3d& p : poly) {
            float dot = p.dot(dir);
            if (dot > maxDot) {
                maxDot = dot;
                supportPoint = p;
            }
        }

        return supportPoint;
    }

    Vector3d supportMinkowski(const std::vector<Vector3d>& polyA, const std::vector<Vector3d>& polyB, Vector3d& dir)
    {
        Vector3d simplexPoint = support(polyA, dir);
        const Vector3d oppositeDir = dir * -1;
        simplexPoint = simplexPoint - support(polyB, oppositeDir);
        return simplexPoint;
    }

    Vector3d checkLineSegment3D(const Vector3d& v0, const Vector3d& v1) {
        Vector3d a = v1 - v0;      // 线段方向向量
        Vector3d b = -v1;          // 从v1到原点的向量
        
        float aDotA = a.dot(a);
        Vector3d bParallel = a * (a.dot(b) / aDotA);  // 平行投影
        Vector3d bPerp = b - bParallel;               // 垂直投影（新方向）
        
        return bPerp;
    }

    Vector3d GetNormalToOri(const Vector3d& a, const Vector3d& b, const Vector3d& c) {
        Vector3d ab = b - a;
        Vector3d ac = c - a;
        Vector3d ad = -a;

        Vector3d n_initial = ab.cross(ac);
        Vector3d n_outer = (n_initial.dot(ad) < 0) ? -n_initial : n_initial;
        return n_outer;
    }

    Vector3d GetDir(const std::vector<Vector3d>& simplex) {
        if (simplex.size() == 1) {
            return -simplex[0];
        } else if (simplex.size() == 2) {
            return checkLineSegment3D(simplex[0], simplex[1]);
        } else if (simplex.size() == 3) {
            return GetNormalToOri(simplex[0], simplex[1], simplex[2]);
        }
        return Vector3d(0, 0, 0);
    }

    bool CheckTetrahedronHaveZero(const std::vector<Vector3d>& simplex) {
        Vector3d A = simplex[0];
        Vector3d B = simplex[1];
        Vector3d C = simplex[2];
        Vector3d D = simplex[3];  // 最新点（面ABC的对
        // 构造线性方程组 Ax = b（求解alpha, beta, gamma）
        Matrix3d mat;
        mat.col(0) = A - D;  // x1-x4, y1-y4, z1-z4
        mat.col(1) = B - D;  // x2-x4, y2-y4, z2-z4
        mat.col(2) = C - D;  // x3-x4, y3-y4, z3-z4
        Vector3d b = -D;     // -x4, -y4, -z4

        // 求解方程组：mat * [alpha; beta; gamma] = b
        if (mat.determinant() == 0) {
            return true;
        }
        Vector3d x = mat.inverse() * b;
        float alpha = x(0), beta = x(1), gamma = x(2);
        float delta = 1 - alpha - beta - gamma;

        // 判定4个权重是否均为正（允许1e-6误差）
        const float eps = 1e-6;
        return (alpha > eps) && (beta > eps) && (gamma > eps) && (delta > eps);
    }

    bool CheckTetrahedronHaveZero2(const std::vector<Vector3d>& simplex) {
        Vector3d A = simplex[0];
        Vector3d B = simplex[1];
        Vector3d C = simplex[2];
        Vector3d D = simplex[3];  // 最新点（面ABC的对
        // 构造线性方程组 Ax = b（求解alpha, beta, gamma）
        auto scalar_triple = [](const Vector3d& a, const Vector3d& b, const Vector3d& c) {
            return a.dot(b.cross(c));  // Eigen封装了叉积和点积，底层运算与手动计算一致
        };

        double v0 = scalar_triple(B, C, D);
        double v1 = scalar_triple(A, C, D);
        double v2 = scalar_triple(A, B, D);
        double v3 = scalar_triple(A, B, C);

        // 判断所有符号是否一致（均正或均负）
        bool all_positive = (v0 > 0) && (v1 > 0) && (v2 > 0) && (v3 > 0);
        bool all_negative = (v0 < 0) && (v1 < 0) && (v2 < 0) && (v3 < 0);

        return all_positive || all_negative;
    }

    bool gjkCollisionJudge(const std::vector<Vector3d>& polyA, const std::vector<Vector3d>& polyB)
    {
        // 初始搜索方向（从A中心指向B中心）
        Vector3d centerA(0, 0, 0), centerB(0, 0, 0);
        for (const auto& p : polyA) {
            centerA = centerA + p;
        }
        for (const auto& p : polyB) {
            centerB = centerB + p;
        }
        centerA = centerA / polyA.size();
        centerB = centerB / polyA.size();
        Vector3d dir = centerB - centerA;

        // 初始化单纯形
        std::vector<Vector3d> simplex;
        Vector3d newPoint = supportMinkowski(polyA, polyB, dir);
        simplex.push_back(newPoint);

        int maxIndex = 3;
        while (true) {
            dir = GetDir(simplex);
            if (dir.dot(dir) < 1e-6) {
                return true;
            }
            newPoint = supportMinkowski(polyA, polyB, dir);
            if (newPoint.dot(newPoint) < 1e-6) {
                return true;
            }
            if (newPoint.dot(dir) < -(1e-6)) {
                return false;
            }
            simplex.push_back(newPoint);
            if (simplex.size() == POLY_SIMPLIFY_MAX_SIZE) {
                bool haveZero = CheckTetrahedronHaveZero2(simplex);
                if (haveZero) {
                    return true;
                } else {
                    float maxLength = 0;
                    for (size_t i = 0; i < POLY_SIMPLIFY_MAX_SIZE; i++) {
                        Vector3d simplexPoint = simplex[i];
                        float newLength = simplexPoint.dot(simplexPoint);
                        if (maxLength < newLength) {
                            maxLength = newLength;
                            maxIndex = i;
                        }
                    }
                    simplex.erase(simplex.begin() + maxIndex);
                }
            }
        }

        return false;
    }

    // 计算OBB在指定轴上的投影范围（min, max）
    std::pair<double, double> projectOBB(const OBB& obb, const Vector3d& axis) {
        // OBB中心在轴上的投影
        double centerProj = obb.center.dot(axis);
        // OBB沿轴的总投影长度（半长在轴上的投影之和）
        double totalExtent = 0.0;
        for (int i = 0; i < 3; ++i) {
            totalExtent += obb.halfExtent[i] * fabs(obb.axis[i].dot(axis));
        }
        return { centerProj - totalExtent, centerProj + totalExtent };
    }

    void getSelfProject(OBB &self) {
        for (int i = 0; i < 3; ++i) {
            auto [min, max] = projectOBB(self, self.axis[i]);
            self.selfMinProject[i] = min;
            self.selfMaxProject[i] = max;
        }
    }

    // 从8个顶点提取OBB参数
    OBB createOBBFromVertices(const std::vector<Eigen::Vector3d>& vertices,
        const float length, const float width, const float height) {
        if (vertices.size() != 8) {
            throw std::invalid_argument("OBB创建失败:必须输入8个顶点");
        }

        OBB obb;

        obb.center = (vertices[1] + vertices[2] + vertices[4] - vertices[0]) * (1.0 / 2.0);

        // 正交化（格拉姆-施密特过程），确保3条轴正交
        obb.axis[0] = (vertices[1] - vertices[0]).normalized();
        obb.axis[1] = (vertices[2] - vertices[0]).normalized();
        obb.axis[2] = (vertices[4] - vertices[0]).normalized();

        obb.halfExtent[0] = length * (1.0 / 2.0);
        obb.halfExtent[1] = width * (1.0 / 2.0);
        obb.halfExtent[2] = height * (1.0 / 2.0);
        obb.maxRadius = 0;
        for (int i = 0; i < 3; ++i) {
            obb.maxRadius += obb.halfExtent[i] * obb.halfExtent[i];
        }
        obb.maxRadius = sqrt(obb.maxRadius);

        getSelfProject(obb);

        return obb;
    }

    OBB createOBBFromPoints(const std::vector<Eigen::Vector3d>& vertices) {
        if (vertices.size() < 8) {
            throw std::invalid_argument("OBB创建失败:必须输入至少8个顶点");
        }

        OBB obb;
        for (int i = 1; i < 8; ++i) {
            Vector3d vec1 = vertices[i] - vertices[0];
            for (int j = i + 1; j < 7; ++j) {
                Vector3d vec2 = vertices[j] - vertices[0];
                if (vec1.dot(vec2) < 1e-4) {
                    for (int k = j + 1; k < 8; ++k) {
                        Vector3d vec3 = vertices[k] - vertices[0];
                        if (fabs(vec1.dot(vec3)) < 1e-4 && fabs(vec2.dot(vec3)) < 1e-4) {
                            // 找到3条互相正交的边
                            obb.axis[0] = vec1.normalized();
                            obb.axis[1] = vec2.normalized();
                            obb.axis[2] = vec3.normalized();

                            // 计算中心点
                            obb.center = (vertices[i] + vertices[j] + vertices[k] - vertices[0]) * (1.0 / 2.0);

                            // 计算半长
                            obb.halfExtent[0] = vec1.norm() * (1.0 / 2.0);
                            obb.halfExtent[1] = vec2.norm() * (1.0 / 2.0);
                            obb.halfExtent[2] = vec3.norm() * (1.0 / 2.0);
                            obb.maxRadius = 0;
                            for (int i = 0; i < 3; ++i) {
                                obb.maxRadius += obb.halfExtent[i] * obb.halfExtent[i];
                            }
                            obb.maxRadius = sqrt(obb.maxRadius);
                            getSelfProject(obb);
                            return obb;
                        }
                    }
                }
            }
        }
        getSelfProject(obb);
        return obb;
    }

    OBB updateOBBFromPoints(OBB originalOBB, Eigen::Vector3d translation, Eigen::Quaterniond rotation) {
        OBB updatedOBB;

        // 更新中心点
        updatedOBB.center = rotation * originalOBB.center + translation;

        // 更新轴方向
        for (int i = 0; i < 3; ++i) {
            updatedOBB.axis[i] = rotation * originalOBB.axis[i];
            updatedOBB.halfExtent[i] = originalOBB.halfExtent[i];
        }
        // getSelfProject(updatedOBB);
        updatedOBB.maxRadius = originalOBB.maxRadius;
        return updatedOBB;
    }

    // 判断两个投影范围是否重叠
    bool areProjectionsOverlapping(double aMin, double aMax, double bMin, double bMax) {
        return !(aMax < bMin - 1e-6 || bMax < aMin - 1e-6);  // 1e-6为浮点误差容限
    }

    static int checkCnt = 0;
    static int checkAxisCnt = 0;
    // OBB碰撞检测主函数（输入：两个长方体的8个顶点，共16个）
    bool checkOBBCollision2(const OBB &obb1, const OBB &obb2) {
        // 其次测试 9 条叉积轴
        checkCnt++;
        checkAxisCnt++;
        double distance = obb1.maxRadius + obb2.maxRadius;
        if (distance * distance < (obb1.center - obb2.center).squaredNorm()) {
            return false;
        }
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                checkAxisCnt++;
                Vector3d crossAxis = obb1.axis[i].cross(obb2.axis[j]);
                double lengthDot = crossAxis.dot(crossAxis);
                if (lengthDot > 1e-6) {  // 排除零向量（轴平行时叉积为零
                    /*if (lengthDot < 25 * 1e-6) {
                        crossAxis = crossAxis.normalized();
                    }*/
                    auto [aMin, aMax] = projectOBB(obb1, crossAxis);
                    auto [bMin, bMax] = projectOBB(obb2, crossAxis);
                    if (aMax < bMin - 1e-6 || bMax < aMin - 1e-6) {
                        // 找到分离轴，判定为不碰撞
                        return false;
                    }
                }
            }
        }

        // 2. 定义所有需要检查的分离轴（共15条）
        // 先测试主轴（更便宜，能更早返回）
        for (int i = 0; i < 3; ++i) {
            checkAxisCnt++;
            auto [bMin, bMax] = projectOBB(obb2, obb1.axis[i]);
            if ((obb1.selfMaxProject[i] < bMin - 1e-6 || bMax < obb1.selfMinProject[i] - 1e-6)){
                // 找到分离轴，判定为不碰撞
                return false;
            }
            checkAxisCnt++;
            auto [aMin, aMax] = projectOBB(obb1, obb2.axis[i]);
            if ((aMax < obb2.selfMinProject[i] - 1e-6 || obb2.selfMaxProject[i] < aMin - 1e-6)){
                // 找到分离轴，判定为不碰撞
                return false;
            }
        }
        
        // 所有轴均重叠，判定为碰撞
        return true;
    }

    bool checkOBBCollision(const OBB &A, const OBB &B) {
        checkCnt++;
        /*double distance = A.maxRadius + B.maxRadius;
        if (distance * distance < (A.center - B.center).squaredNorm()) {
            checkAxisCnt += 1;
            return false;
        }*/
        // Broadphase: sphere check (optional, skipped for determinism)
        const double eps = 1e-6;

        // Half extents
        const double a0 = A.halfExtent[0], a1 = A.halfExtent[1], a2 = A.halfExtent[2];
        const double b0 = B.halfExtent[0], b1 = B.halfExtent[1], b2 = B.halfExtent[2];

        // Rotation matrix expressing B in A's frame: R[i][j] = Ai · Bj
        double R[3][3];
        double absR[3][3];
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                R[i][j] = A.axis[i].dot(B.axis[j]);
                absR[i][j] = std::abs(R[i][j]) + eps;
            }
        }

        // Translation vector from A to B in world, then expressed in A's frame
        Eigen::Vector3d tWorld = B.center - A.center;
        double tA[3] = { tWorld.dot(A.axis[0]), tWorld.dot(A.axis[1]), tWorld.dot(A.axis[2]) };

        // Test axes L = Ai x Bj
        // A0 x B0
        {
            double ra = a1 * absR[2][0] + a2 * absR[1][0];
            double rb = b1 * absR[0][2] + b2 * absR[0][1];
            double t = std::abs(tA[2] * R[1][0] - tA[1] * R[2][0]);
            checkAxisCnt ++;
            printf("A0 x B0: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A0 x B1
        {
            double ra = a1 * absR[2][1] + a2 * absR[1][1];
            double rb = b0 * absR[0][2] + b2 * absR[0][0];
            double t = std::abs(tA[2] * R[1][1] - tA[1] * R[2][1]);
            checkAxisCnt ++;
            printf("A0 x B1: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A0 x B2
        {
            double ra = a1 * absR[2][2] + a2 * absR[1][2];
            double rb = b0 * absR[0][1] + b1 * absR[0][0];
            double t = std::abs(tA[2] * R[1][2] - tA[1] * R[2][2]);
            checkAxisCnt ++;
            printf("A0 x B2: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A1 x B0
        {
            double ra = a0 * absR[2][0] + a2 * absR[0][0];
            double rb = b1 * absR[1][2] + b2 * absR[1][1];
            double t = std::abs(tA[0] * R[2][0] - tA[2] * R[0][0]);
            checkAxisCnt ++;
            printf("A1 x B0: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A1 x B1
        {
            double ra = a0 * absR[2][1] + a2 * absR[0][1];
            double rb = b0 * absR[1][2] + b2 * absR[1][0];
            double t = std::abs(tA[0] * R[2][1] - tA[2] * R[0][1]);
            checkAxisCnt ++;
            printf("A1 x B1: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A1 x B2
        {
            double ra = a0 * absR[2][2] + a2 * absR[0][2];
            double rb = b0 * absR[1][1] + b1 * absR[1][0];
            double t = std::abs(tA[0] * R[2][2] - tA[2] * R[0][2]);
            checkAxisCnt ++;
            printf("A1 x B2: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A2 x B0
        {
            double ra = a0 * absR[1][0] + a1 * absR[0][0];
            double rb = b1 * absR[2][2] + b2 * absR[2][1];
            double t = std::abs(tA[1] * R[0][0] - tA[0] * R[1][0]);
            checkAxisCnt ++;
            printf("A2 x B0: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A2 x B1
        {
            double ra = a0 * absR[1][1] + a1 * absR[0][1];
            double rb = b0 * absR[2][2] + b2 * absR[2][0];
            double t = std::abs(tA[1] * R[0][1] - tA[0] * R[1][1]);
            checkAxisCnt ++;
            printf("A2 x B1: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }
        // A2 x B2
        {
            double ra = a0 * absR[1][2] + a1 * absR[0][2];
            double rb = b0 * absR[2][1] + b1 * absR[2][0];
            double t = std::abs(tA[1] * R[0][2] - tA[0] * R[1][2]);
            checkAxisCnt ++;
            printf("A2 x B2: t = %f, ra + rb = %f\n", t, ra + rb);
            if (t > ra + rb) return false;
        }

        // Test axes L = A0, A1, A2
        {
            double ra = a0;
            double rb = b0 * absR[0][0] + b1 * absR[0][1] + b2 * absR[0][2];
            checkAxisCnt ++;
            printf("A0 x B0: t = %f, ra + rb = %f\n", tA[0], ra + rb);
            if (std::abs(tA[0]) > ra + rb) return false;
        }
        {
            double ra = a1;
            double rb = b0 * absR[1][0] + b1 * absR[1][1] + b2 * absR[1][2];
            checkAxisCnt ++;
            printf("A1 x B1: t = %f, ra + rb = %f\n", tA[1], ra + rb);
            if (std::abs(tA[1]) > ra + rb) return false;
        }
        {
            double ra = a2;
            double rb = b0 * absR[2][0] + b1 * absR[2][1] + b2 * absR[2][2];
            checkAxisCnt ++;
            printf("A2 x B2: t = %f, ra + rb = %f\n", tA[2], ra + rb);
            if (std::abs(tA[2]) > ra + rb) return false;
        }

        // Test axes L = B0, B1, B2
        {
            double ra = a0 * absR[0][0] + a1 * absR[1][0] + a2 * absR[2][0];
            double rb = b0;
            double tB = std::abs(tWorld.dot(B.axis[0]));
            checkAxisCnt ++;
            printf("B0: t = %f, ra + rb = %f\n", tB, ra + rb);
            if (tB > ra + rb) return false;
        }
        {
            double ra = a0 * absR[0][1] + a1 * absR[1][1] + a2 * absR[2][1];
            double rb = b1;
            double tB = std::abs(tWorld.dot(B.axis[1]));
            checkAxisCnt ++;
            printf("B1: t = %f, ra + rb = %f\n", tB, ra + rb);
            if (tB > ra + rb) return false;
        }
        {
            double ra = a0 * absR[0][2] + a1 * absR[1][2] + a2 * absR[2][2];
            double rb = b2;
            double tB = std::abs(tWorld.dot(B.axis[2]));
            checkAxisCnt ++;
            printf("B2: t = %f, ra + rb = %f\n", tB, ra + rb);
            if (tB > ra + rb) return false;
        }

        return true;
    }

    void GetAxisLog(double &checkAxisAve) {
        checkAxisAve = static_cast<double>(checkAxisCnt) / static_cast<double>(checkCnt);
        checkAxisCnt = 0;
        checkCnt = 0;
    }

    void BubbleSortAxisByHitCount(AxisStats *stats) {
        if (stats == NULL) {
            return;
        }

        for (int i = 0; i < AXIS_CNT; ++i) {
            if (stats->axisRank[i] == AXIS_DISTANCE_LONG) {
                std::swap(stats->axisRank[i], stats->axisRank[0]);
            }
            stats->hitCount[i] = stats->hitCount[i] / 2;
        }
        
        for (int i = 1; i < AXIS_CNT - 1; ++i) {
            bool swapped = false;
            for (int j = 1; j < AXIS_CNT - 1 - i; ++j) {
                int currAxis = stats->axisRank[j];
                int nextAxis = stats->axisRank[j + 1];
                if (stats->hitCount[currAxis] < stats->hitCount[nextAxis]) {
                    std::swap(stats->axisRank[j], stats->axisRank[j + 1]);
                    swapped = true;
                }
            }
            if (!swapped) {
                break;
            }
        }
    }

    using CheckCollisionAxisFunc2 = bool(*)
        (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats);
    
    bool checkCollisionAxisDistance2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double distance = obb1.maxRadius + obb2.maxRadius;
        return distance * distance > tWorld.squaredNorm();
    }

    bool checkCollisionAxisOBB2_A_Axis_0
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0]));
        double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1]));
        double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[0];
        double rb = obb2.halfExtent[0] * r00 + obb2.halfExtent[1] * r01 + obb2.halfExtent[2] * r02;
        double t = std::abs(tWorld.dot(obb1.axis[0]));
        return t < ra + rb;
    }

    bool checkCollisionAxisOBB2_A_Axis_1
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0]));
        double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1]));
        double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[1];
        double rb = obb2.halfExtent[0] * r10 + obb2.halfExtent[1] * r11 + obb2.halfExtent[2] * r12;
        double t = std::abs(tWorld.dot(obb1.axis[1]));
        return t < ra + rb;
    }

    bool checkCollisionAxisOBB2_A_Axis_2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0]));
        double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1]));
        double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[2];
        double rb = obb2.halfExtent[0] * r20 + obb2.halfExtent[1] * r21 + obb2.halfExtent[2] * r22;
        double t = std::abs(tWorld.dot(obb1.axis[2]));
        return t < ra + rb;
    }

    bool checkCollisionAxisOBB2_B_Axis_0
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r00 = std::abs(obb2.axis[0].dot(obb1.axis[0]));
        double r10 = std::abs(obb2.axis[0].dot(obb1.axis[1]));
        double r20 = std::abs(obb2.axis[0].dot(obb1.axis[2]));
        double ra = obb1.halfExtent[0] * r00 + obb1.halfExtent[1] * r10 + obb1.halfExtent[2] * r20;
        double rb = obb2.halfExtent[0];
        double t = std::abs(tWorld.dot(obb2.axis[0]));
        return t < ra + rb;
    }

    bool checkCollisionAxisOBB2_B_Axis_1
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r01 = std::abs(obb2.axis[1].dot(obb1.axis[0]));
        double r11 = std::abs(obb2.axis[1].dot(obb1.axis[1]));
        double r21 = std::abs(obb2.axis[1].dot(obb1.axis[2]));
        double ra = obb1.halfExtent[0] * r01 + obb1.halfExtent[1] * r11 + obb1.halfExtent[2] * r21;
        double rb = obb2.halfExtent[1];
        double t = std::abs(tWorld.dot(obb2.axis[1]));
        return t < ra + rb;
    }

    bool checkCollisionAxisOBB2_B_Axis_2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r02 = std::abs(obb2.axis[2].dot(obb1.axis[0]));
        double r12 = std::abs(obb2.axis[2].dot(obb1.axis[1]));
        double r22 = std::abs(obb2.axis[2].dot(obb1.axis[2]));
        double ra = obb1.halfExtent[0] * r02 + obb1.halfExtent[1] * r12 + obb1.halfExtent[2] * r22;
        double rb = obb2.halfExtent[2];
        double t = std::abs(tWorld.dot(obb2.axis[2]));
        return t < ra + rb;
    }

    bool checkCollisionAxis2A0_Cross_B0
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0]));
        double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0]));
        double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1]));
        double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[2] * r10 + obb1.halfExtent[1] * r20;
        double rb = obb2.halfExtent[2] * r01 + obb2.halfExtent[1] * r02;
        double t = std::abs(tWorld.dot(obb1.axis[2]) * r10 - tWorld.dot(obb1.axis[1]) * r20);
        return t < ra + rb;
    }   

    bool checkCollisionAxis2A0_Cross_B1
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1]));
        double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1]));
        double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2]));
        double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0]));
        double ra = obb1.halfExtent[2] * r11 + obb1.halfExtent[1] * r21;
        double rb = obb2.halfExtent[0] * r02 + obb2.halfExtent[2] * r00;
        double t = std::abs(tWorld.dot(obb1.axis[2]) * r11 - tWorld.dot(obb1.axis[1]) * r21);
        return t < ra + rb;
    }

    bool checkCollisionAxis2A0_Cross_B2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2]));
        double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2]));
        double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1]));
        double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0]));
        double ra = obb1.halfExtent[2] * r12 + obb1.halfExtent[1] * r22;
        double rb = obb2.halfExtent[0] * r01 + obb2.halfExtent[1] * r00;
        double t = std::abs(tWorld.dot(obb1.axis[2]) * r12 - tWorld.dot(obb1.axis[1]) * r22);
        return t < ra + rb;
    }

    bool checkCollisionAxis2A1_Cross_B0
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0]));
        double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0]));
        double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1]));
        double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[0] * r20 + obb1.halfExtent[2] * r00;
        double rb = obb2.halfExtent[2] * r11 + obb2.halfExtent[1] * r12;
        double t = std::abs(tWorld.dot(obb1.axis[0]) * r20 - tWorld.dot(obb1.axis[2]) * r00);
        return t < ra + rb;
    }
    
    bool checkCollisionAxis2A1_Cross_B1
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1]));
        double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1]));
        double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2]));
        double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0]));
        double ra = obb1.halfExtent[0] * r21 + obb1.halfExtent[2] * r01;
        double rb = obb2.halfExtent[0] * r12 + obb2.halfExtent[2] * r10;
        double t = std::abs(tWorld.dot(obb1.axis[0]) * r21 - tWorld.dot(obb1.axis[2]) * r01);
        return t < ra + rb;
    }
    
    bool checkCollisionAxis2A1_Cross_B2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2]));
        double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2]));
        double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1]));
        double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0]));
        double ra = obb1.halfExtent[0] * r22 + obb1.halfExtent[2] * r02;
        double rb = obb2.halfExtent[0] * r11 + obb2.halfExtent[2] * r10;
        double t = std::abs(tWorld.dot(obb1.axis[0]) * r22 - tWorld.dot(obb1.axis[2]) * r02);
        return t < ra + rb;
    }
    
    bool checkCollisionAxis2A2_Cross_B0
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0]));
        double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0]));
        double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1]));
        double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2]));
        double ra = obb1.halfExtent[1] * r00 + obb1.halfExtent[0] * r10;
        double rb = obb2.halfExtent[2] * r21 + obb2.halfExtent[1] * r22;
        double t = std::abs(tWorld.dot(obb1.axis[1]) * r00 - tWorld.dot(obb1.axis[0]) * r10);
        return t < ra + rb;
    }
    
    bool checkCollisionAxis2A2_Cross_B1
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1]));
        double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1]));
        double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2]));
        double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0]));
        double ra = obb1.halfExtent[1] * r01 + obb1.halfExtent[0] * r11;
        double rb = obb2.halfExtent[0] * r22 + obb2.halfExtent[2] * r20;
        double t = std::abs(tWorld.dot(obb1.axis[1]) * r01 - tWorld.dot(obb1.axis[0]) * r11);
        return t < ra + rb;
    }
    
    bool checkCollisionAxis2A2_Cross_B2
    (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) {
        UNUSED(stats);
        double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2]));
        double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2]));
        double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0]));
        double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1]));
        double ra = obb1.halfExtent[1] * r02 + obb1.halfExtent[0] * r12;
        double rb = obb2.halfExtent[1] * r20 + obb2.halfExtent[0] * r21;
        double t = std::abs(tWorld.dot(obb1.axis[1]) * r02 - tWorld.dot(obb1.axis[0]) * r12);
        return t < ra + rb;
    }

    CheckCollisionAxisFunc2 checkCollisionAxisFunc2[AXIS_CNT] = {
        checkCollisionAxisDistance2,
        checkCollisionAxisOBB2_A_Axis_0,
        checkCollisionAxisOBB2_A_Axis_1,
        checkCollisionAxisOBB2_A_Axis_2,
        checkCollisionAxisOBB2_B_Axis_0,
        checkCollisionAxisOBB2_B_Axis_1,
        checkCollisionAxisOBB2_B_Axis_2,
        checkCollisionAxis2A0_Cross_B0,
        checkCollisionAxis2A0_Cross_B1,
        checkCollisionAxis2A0_Cross_B2,
        checkCollisionAxis2A1_Cross_B0,
        checkCollisionAxis2A1_Cross_B1,
        checkCollisionAxis2A1_Cross_B2,
        checkCollisionAxis2A2_Cross_B0,
        checkCollisionAxis2A2_Cross_B1,
        checkCollisionAxis2A2_Cross_B2,
    };

    bool checkOBBCollisionNew(const OBB &A, const OBB &B, AxisStats *stats) {
        checkCnt++;
        Vector3d tWorld = B.center - A.center;

        for (int axisIndex = 1; axisIndex < AXIS_CNT; ++axisIndex) {
            checkAxisCnt++;
            int axis = stats->axisRank[0];
            if (!checkCollisionAxisFunc2[axis](A, B, tWorld, stats)) {
                stats->hitCount[axis]++;
                return false;
            } else {
                stats->hitCount[axis]--;
                std::swap(stats->axisRank[0], stats->axisRank[axisIndex]);
            }
        }
        checkAxisCnt++;
        return checkCollisionAxisFunc2[stats->axisRank[0]](A, B, tWorld, stats);
    }
}
