/*********************************************
 *
 * Cedric Pradalier
 * DEA 2000/2001 
 * INRIA Rhones Alpes
 * http://cedric.pradalier.free.fr/index.html
 * mail : http://cedric.pradalier.free.fr/mail.html
 *
 * *******************************************/


#include <math.h>
#include <stdlib.h>
#include "Segment.h"


/*
 * System : direction = (cos(theta),sin(theta))
 * { org.x + lambda*cos(theta) = A.x + mu*(B.x - A.x)
 * { org.y + lambda*sin(theta) = A.y + mu*(B.y - A.y)
 * */
double Segment::intersect(const Vector2 & org,double theta)
{
    /* det is never null */
    double a = cos(theta);
    double b = A.getX() - B.getX();
    double c = sin(theta) ;
    double d =  A.getY() - B.getY();
    double det = a * d - b * c;
    if (det == 0.0) det = 1e-15;
    double rhs1 = A.getX() - org.getX();
    double rhs2 = A.getY() - org.getY();
    double lambda = (d * rhs1 - b * rhs2)/det;
    double mu = (-c * rhs1 + a * rhs2)/det;

    //printf("Inter : Lambda %e | Mu %e\n",lambda,mu);
    /* The point is on the line but not on the segment */
    if ((mu < 0.0) || (mu > 1.0)) return -1.0;
    
    /* The point is not at the good side of org */
    if (lambda < 0) return -1.0;
    
    return lambda;
}

/*
 * System : direction = (cos(theta),sin(theta))
 * { org.x + lambda*dir.x = A.x + mu*(B.x - A.x)
 * { org.y + lambda*dir.y = A.y + mu*(B.y - A.y)
 * */
double Segment::intersect(const Vector2 & org,const Vector2 & dir,double * mu)
{
    /* det is never null */
    double dA = (org - A).norme();
    double dB = (org - B).norme();
    double l = (A - B).norme();
    if (fabs((dA+dB)/l - 1) < 1e-3) 
    {
        *mu = dA;
        return 0;
    }
        
    double a = dir.x;
    double b = A.getX() - B.getX();
    double c = dir.y;
    double d =  A.getY() - B.getY();
    double det = a * d - b * c;
    if (det == 0.0) {/*printf("det = 1e-20\n");*/det = 1e-20;}
    double rhs1 = A.getX() - org.getX();
    double rhs2 = A.getY() - org.getY();
    double lambda = (d * rhs1 - b * rhs2)/det;
    *mu = (-c * rhs1 + a * rhs2)/det;

    //printf("Inter : Lambda %e | Mu %e\n",lambda,mu);
    /* The point is on the line but not on the segment */
    if ((*mu < 0.0) || (*mu > 1.0)) return -1.0;
    
    /* The point is not on the good side of org */
    if (lambda < 0) return -1.0;
    
    return lambda;
}


Vector2 Segment::intersectNoCheck(const Segment & S2)
{
    /* det is never null */
    Vector2 dirS2 = (S2.B - S2.A).unitaire();
    
    double a = dirS2.getX();
    double b = A.getX() - B.getX();
    double c = dirS2.getY() ;
    double d =  A.getY() - B.getY();
    double det = a * d - b * c;
    if (det == 0.0) det = 1e-15;
    double rhs1 = A.getX() - S2.A.getX();
    double rhs2 = A.getY() - S2.A.getY();
    double lambda = (d * rhs1 - b * rhs2)/det;

    
//  printf("Intersection : %f\n",lambda);
    return S2.A + dirS2 * lambda;
}