/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "proximity.h"
#include <sofa/helper/LCPSolver.inl>

namespace sofa
{

namespace helper
{
using namespace sofa::helper;
using namespace sofa::defaulttype;

//------------------------
// DISTANCETRITRI
/*
 aP = alphaP, bP = betaP are the barycentric coordinates of point P on triangle P
 aQ = alphaQ, bQ = betaQ are the barycentric coordinates of point Q on triangle Q

 the computation of the distance between P and Q can be done with:

QP^2 = [aP bP aQ bQ] * [P1P2*P1P2 P1P2*P1P3 -P1P2*Q1Q2 -P1P2*Q1Q3 * [aP bP aQ bQ]^T
								   P1P3*P1P3 -P1P3*Q1Q2 -P1P3*Q1Q3
						sym					 Q1Q2*Q1Q2  Q1Q2*Q1Q3
													    Q1Q3*Q1Q3]
  + [Q1P1*P1P2 Q1P1*P1P3 -Q1P1*P1Q2 -Q1P1*P1Q3] *[aP bP aQ bQ]^T + (Q1P1)^2

  P and Q are on triangles, so we have the constraints:
aP>=0, bP>=0, aQ>=0, bQ>=0
aP+bP<=1, aQ+bQ<=1 <=> -aP-bP>=-1 , -aQ-bQ>=-1

To perform min(QP^2) we use LCP formalism

*/
//------------------------

DistanceTriTri::
DistanceTriTri()
{
}

DistanceTriTri::
~DistanceTriTri()
{
}



void DistanceTriTri::
NewComputation(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, Vector3 &Presult, Vector3 &Qresult)
{
    //Vector3 P1, P2, P3, Q1, Q2, Q3;
    //P1 = triP->p1();
    //P2 = triP->p2();
    //P3 = triP->p3();
    //Q1 = triQ->p1();
    //Q2 = triQ->p2();
    //Q3 = triQ->p3();

    double alphaP, betaP, alphaQ, betaQ;
    int i;
    Vector3 P1P2, P1P3, Q1Q2, Q1Q3, P1Q1, PQ;
    LCPSolver<6>::Matrix  A;
    double          b[6], result[12];
    LCPSolver<6>          lcp;

    // clear result
    for (i=0; i<12; i++)
    {
        result[i] = 0.0;
    }


    P1P2=P2 - P1;
    P1P3=P3 - P1;
    Q1Q2=Q2 - Q1;
    Q1Q3=Q3 - Q1;
    P1Q1=Q1 - P1;

    A[0][4] = 1.0; A[4][0] = -1.0;
    A[1][4] = 1.0; A[4][1] = -1.0;
    A[2][4] = 0.0; A[4][2] = 0.0;
    A[3][4] = 0.0; A[4][3] = 0.0;

    A[0][5] = 0.0; A[5][0] = 0.0;
    A[1][5] = 0.0; A[5][1] = 0.0;
    A[2][5] = 1.0; A[5][2] = -1.0;
    A[3][5] = 1.0; A[5][3] = -1.0;

    A[4][4] = 0.0; A[5][5] = 0.0;
    A[4][5] = 0.0; A[5][4] = 0.0;

    A[0][0] = dot(P1P2,P1P2);   A[0][1] = dot(P1P3,P1P2);   A[0][2] =-dot(Q1Q2,P1P2);  A[0][3] =-dot(Q1Q3,P1P2);
    A[1][0] = dot(P1P2,P1P3);   A[1][1] = dot(P1P3,P1P3);   A[1][2] =-dot(Q1Q2,P1P3);  A[1][3] =-dot(Q1Q3,P1P3);
    A[2][0] = -dot(P1P2,Q1Q2);  A[2][1] = -dot(P1P3,Q1Q2);  A[2][2] = dot(Q1Q2,Q1Q2);  A[2][3] = dot(Q1Q3,Q1Q2);
    A[3][0] = -dot(P1P2,Q1Q3);  A[3][1] = -dot(P1P3,Q1Q3);  A[3][2] = dot(Q1Q2,Q1Q3);  A[3][3] = dot(Q1Q3,Q1Q3);

    b[0]=-dot(P1Q1,P1P2);  b[1]=-dot(P1Q1,P1P3);  b[2]= dot(P1Q1,Q1Q2); b[3]= dot(P1Q1,Q1Q3);
    b[4]=1.0;  b[5]=1.0;

    if(lcp.solve(b, A, result)==1)
    {
        alphaP=result[6];
        betaP=result[7];
        alphaQ=result[8];
        betaQ=result[9];
        Presult = P1 + P1P2*alphaP + P1P3*betaP;
        Qresult = Q1 + Q1Q2*alphaQ + Q1Q3*betaQ;
    }
    else
    {
        printf(" no result from LCP !\n");
        Presult = P1;
        Qresult = Q1;
    }


}


//------------------------
// DISTANCESEGTRI
//------------------------

DistanceSegTri::
DistanceSegTri()
{
}

DistanceSegTri::
~DistanceSegTri()
{
}

//void DistanceSegTri::
//NewComputation(Triangle *tri, const Vector3 &Q1, const Vector3 &Q2, Vector3 &Presult, Vector3 &Qresult)
//{
//	NewComputation( tri->p1(), tri->p2(), tri->p3(), Q1, Q2,Presult, Qresult );
//}

void DistanceSegTri::
NewComputation(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &Q1, const Vector3 &Q2, Vector3 &Presult, Vector3 &Qresult)
{
    double alpha, beta, gamma;
    int i;


    Vector3 P1P2, P1P3, Q1Q2, P1Q1, PQ;
    LCPSolver<5>::Matrix  A;
    double          b[5], result[10];
    LCPSolver<5>          lcp;

    for (i=0; i<10; i++)
    {
        result[i] = 0.0;
    }

    Q1Q2 = Q2 - Q1;
    P1P2 = P2 - P1;
    P1P3 = P3 - P1;
    P1Q1 = Q1 - P1;


    // initialize A
    A[0][3] = 1.0; A[0][4] = 0.0;
    A[1][3] = 1.0; A[1][4] = 0.0;
    A[2][3] = 0.0; A[2][4] = 1.0;
    A[3][0] = -1.0; A[3][1] = -1.0; A[3][2] = 0.0;	A[3][3] = 0.0; A[3][4] = 0.0;
    A[4][0] = 0.0; A[4][1] = 0.0;	A[4][2] = -1.0;	A[4][3] = 0.0; A[4][4] = 0.0;

    A[0][0] = dot(P1P2,P1P2);   A[0][1] = dot(P1P3,P1P2);   A[0][2] = -dot(Q1Q2,P1P2);
    A[1][0] = dot(P1P2,P1P3);   A[1][1] = dot(P1P3,P1P3);   A[1][2] = -dot(Q1Q2,P1P3);
    A[2][0] = -dot(P1P2,Q1Q2);  A[2][1] = -dot(P1P3,Q1Q2);  A[2][2] = dot(Q1Q2,Q1Q2);

    // initialize b
    b[3]=1.0;  b[4]=1.0;
    b[0]=-dot(P1Q1,P1P2);  b[1]=-dot(P1Q1,P1P3);  b[2]=dot(P1Q1,Q1Q2);

    if(lcp.solve(b, A, result))
    {
        alpha=result[5];
        beta=result[6];
        gamma=result[7];

        Presult = P1 + P1P2*alpha + P1P3*beta;
        Qresult = Q1 + Q1Q2*gamma;
    }
    else
    {
        printf(" no result from LCP !\n");
//        alpha=0;
//        beta=0;
//        gamma=0;
        Presult = P1;
        Qresult = Q1;
    }

}

//------------------------------------------------------------------------------------------------------ //
// ---------------------------------------- DISTANCEPOINTTRI ------------------------------------------- //
//------------------------------------------------------------------------------------------------------ //


DistancePointTri::
DistancePointTri()
{
}

DistancePointTri::
~DistancePointTri()
{
}

void DistancePointTri::
NewComputation(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &Q, Vector3 &Presult)
{
    double alpha, beta;
    int i;

    //Vector3 P1, P2, P3;

    Vector3 P1P2, P1P3, P1Q;
    LCPSolver<3>::Matrix  A;
    double          b[3], result[6];
    LCPSolver<3>    lcp;

    //P1 = tri->p1();
    //P2 = tri->p2();
    //P3 = tri->p3();


    for (i=0; i<6; i++)
    {
        result[i] = 0.0;
    }

    P1P2 = P2 - P1;
    P1P3 = P3 - P1;
    P1Q = Q - P1;

    // initialize A
    A[0][2] = 1.0;
    A[1][2] = 1.0;
    A[2][0] = -1.0; A[2][1] = -1.0; A[2][2] = 0.0;
    A[0][0] = dot(P1P2,P1P2);   A[0][1] = dot(P1P3,P1P2);
    A[1][0] = dot(P1P2,P1P3);   A[1][1] = dot(P1P3,P1P3);

    // initialize b
    b[2]=1.0;
    b[0]=-dot(P1Q,P1P2);  b[1]=-dot(P1Q,P1P3);

    if(lcp.solve(b, A, result))
    {
        alpha=result[3];
        beta=result[4];

        Presult = P1 + P1P2*alpha + P1P3*beta;
    }
    else
    {
        printf(" no result from LCP !\n");
//        alpha=0;
//        beta=0;
        Presult = P1;
    }
}


} // namespace helper

} // namespace sofa
