#include "CGjk.h"

CGjk::CGjk() {
    m_psfpSimplexUpdateFuncs[SIMPLEX::ST_EDGE] = &CGjk::SimplexEdgeUpdate;
    m_psfpSimplexUpdateFuncs[SIMPLEX::ST_FACE] = &CGjk::SimplexFaceUpdate;
    m_psfpSimplexUpdateFuncs[SIMPLEX::ST_TETRAHEDRON] = &CGjk::SimplexTetrahedronUpdate;
}

CGjk::~CGjk() {
}

void CGjk::GetSupportPoint(const CVector3& _vSearchDir, const CShapeInstance* _psiLeft, const CShapeInstance* _psiRight, SIMPLEX_VERTEX& _vRet) const {
    CVector3 vSupA;
    _vRet.ui32SupA = (const CPolyhedron*)(_psiLeft->ShapeInstanceShape())->GetSupport(_vSearchDir, vSupA);
    CVector3 vSupB;
    _vRet.ui32SupB = (const CPolyhedron*)(_psiRight->ShapeInstanceShape())->GetSupport(-_vSearchDir, vSupB);
    _vRet.vSupA = vSupA;
    _vRet.vSupB = vSupB;
    _vRet.vMinkSup = _vRet.vSupA - _vRet.vSupB;
}

bool CGjk::Intersect(const CShapeInstance* _psiLeft, const CShapeInstance* _psiRight) {
    m_vSearchDir = CVector3( ML_ONE, ML_ONE, ML_ONE );

    //vA it's our first supporting point.
    GetSupportPoint(m_vSearchDir, _psiLeft, _psiRight, m_sSimplex.vA);
    m_sSimplex.ssSimplexType = SIMPLEX::ST_POINT;

    m_vSearchDir = -m_vSearchDir;

    for ( unsigned int I = 0UL; I < GJK_ITERATIONS; ++I ) {
        SIMPLEX_VERTEX vNewSimplexVertex;
        GetSupportPoint(m_vSearchDir, _psiLeft, _psiRight, vNewSimplexVertex);

        if (vNewSimplexVertex.vMinkSup.Dot(m_vSearchDir) <= ML_ZERO) { return false; }

        //Add new point to create a new simplex.
        if (m_sSimplex.ssSimplexType == SIMPLEX::ST_POINT) {
            m_sSimplex.vB = m_sSimplex.vA;
            m_sSimplex.vA = vNewSimplexVertex;
            m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
        }
        else if (m_sSimplex.ssSimplexType == SIMPLEX::ST_EDGE) {
            m_sSimplex.vC = m_sSimplex.vB;
            m_sSimplex.vB = m_sSimplex.vA;
            m_sSimplex.vA = vNewSimplexVertex;
            m_sSimplex.ssSimplexType = SIMPLEX::ST_FACE;
        }
        else if (m_sSimplex.ssSimplexType == SIMPLEX::ST_FACE) {
            m_sSimplex.vD = m_sSimplex.vC;
            m_sSimplex.vC = m_sSimplex.vB;
            m_sSimplex.vB = m_sSimplex.vA;
            m_sSimplex.vA = vNewSimplexVertex;
            m_sSimplex.ssSimplexType = SIMPLEX::ST_TETRAHEDRON;
        }

        //Check if the simplex contains the origin.
        //Update simplex and direction.
        if ( UpdateSimplex() ) { return true; }
    }

    return false;
}

bool CGjk::UpdateSimplex() {
    return (this->*m_psfpSimplexUpdateFuncs[m_sSimplex.ssSimplexType])();
}

bool CGjk::SimplexEdgeUpdate() {
    CVector3 vAo = -m_sSimplex.vA.vMinkSup;
    CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
    m_vSearchDir = vAb.Cross(vAo).Cross(vAb);
    return false;
}

bool CGjk::SimplexFaceUpdate() {
    CVector3 vAo = -m_sSimplex.vA.vMinkSup;
    CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
    CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
    CVector3 vFaceNormal = vAb.Cross(vAc);

    if ( vAb.Cross(vFaceNormal).Dot(vAo) > ML_ZERO ) {
        m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
        //A and B makes the edge.
        m_vSearchDir = vAb.Cross(vAo).Cross(vAb); //Ab to Ao.
        return false;
    }

    if ( vFaceNormal.Cross(vAc).Dot(vAo) > ML_ZERO ) {
        //A and B makes the edge.
        m_sSimplex.vB = m_sSimplex.vC;
        m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
        m_vSearchDir = vAc.Cross(vAo).Cross(vAc); //Ac to Ao.
        return false;
    }

    if ( vFaceNormal.Dot(vAo) > ML_ZERO ) {
        //Above face.
        m_vSearchDir = vFaceNormal;
        return false;
    }

    //Below face.
    CMathLib::Swap(m_sSimplex.vB, m_sSimplex.vC);
    m_vSearchDir = -vFaceNormal;
    return false;
}

bool CGjk::SimplexTetrahedronUpdate() {
    CVector3 vAo = -m_sSimplex.vA.vMinkSup;

    CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
    CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
    if ( vAb.Cross(vAc).Dot(vAo) > ML_ZERO ) {
        return UpdateSimplexTetrahedronFace(vAo);
    }

    CVector3 vAd = m_sSimplex.vD.vMinkSup - m_sSimplex.vA.vMinkSup;
    if ( vAc.Cross(vAd).Dot(vAo) > ML_ZERO ) {
        m_sSimplex.vB = m_sSimplex.vC;
        m_sSimplex.vC = m_sSimplex.vD;
        return UpdateSimplexTetrahedronFace(vAo);
    }

    if ( vAd.Cross(vAb).Dot(vAo) > ML_ZERO ) {
        SIMPLEX_VERTEX vOldB = m_sSimplex.vB;
        m_sSimplex.vB = m_sSimplex.vD;
        m_sSimplex.vC = vOldB;
        return UpdateSimplexTetrahedronFace(vAo);
    }

    return true;
}

bool CGjk::UpdateSimplexTetrahedronFace(const CVector3& _vAo) {
    CVector3 vAb = m_sSimplex.vB.vMinkSup - m_sSimplex.vA.vMinkSup;
    CVector3 vAc = m_sSimplex.vC.vMinkSup - m_sSimplex.vA.vMinkSup;
    CVector3 vFaceNormal = vAb.Cross(vAc);

    if (vAb.Cross(vFaceNormal).Dot(_vAo) > ML_ZERO) {
        //Test if origin it is in the voronoi region of the AB edge.
        m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
        //Get the perpendicular direction of the edge towards the origin.
        m_vSearchDir = vAb.Cross(_vAo).Cross(vAb);
        return false;
    }

    if ( vFaceNormal.Cross(vAc).Dot(_vAo) > ML_ZERO ) {
        //Test if origin it is in the voronoi region of the AC edge.
        //Change to edge.
        m_sSimplex.vB = m_sSimplex.vC;
        m_sSimplex.ssSimplexType = SIMPLEX::ST_EDGE;
        //Get the perpendicular direction of the edge towards the origin.
        m_vSearchDir = vAc.Cross(_vAo).Cross(vAc);
        return false;
    }

    m_sSimplex.ssSimplexType = SIMPLEX::ST_FACE;
    m_vSearchDir = vFaceNormal;

    return false;
}