/*  $Id: line_math.cpp,v 1.4 2004/04/20 13:36:27 sphair Exp $
**
**  ClanLib Game SDK
**  Copyright (C) 2003  The ClanLib Team
**  For a total list of contributers see the file CREDITS.
**
**  This library 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 library 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 library; if not, write to the Free Software
**  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
**
*/

#include "Core/precomp.h"
#include <cmath>
#include "API/Core/Math/line_math.h"

template<typename T> inline T pow2(T value) { return value*value; }
template<typename T> inline T cl_min(T a, T b) { if(a < b) return a; return b; }
template<typename T> inline T cl_max(T a, T b) { if(a > b) return a; return b; }

/* ----- from comp.graphics.algorithms FAQ ------

  Distance to line (Ax,Ay->Bx,By) from point x,y

	L = sqrt( (Bx-Ax)^2 + (By-Ay)^2 )

	    (Cx-Ax)(Bx-Ax) + (Cy-Ay)(By-Ay)
	r = -------------------------------
	                 L^2
*/

float CL_LineMath::distance_to_line(const CL_Pointf &P, float *line)
{
	return distance_to_line( P.x, P.y, line );
}

float CL_LineMath::distance_to_line(float x, float y, float *line)
{
	const float &Ax = line[0];
	const float &Ay = line[1];
	const float &Bx = line[2];
	const float &By = line[3];

	float L = sqrt( pow2(Bx-Ax) + pow2(By-Ay) );
	float r = ((x-Ax)*(Bx-Ax)+(y-Ay)*(By-Ay))/pow2(L);
	
	if( r <= 0 || r >= 1 )
	{
		CL_Pointf p(x,y);
		CL_Pointf A(Ax,Ay);
		CL_Pointf B(Bx,By);
		
		return cl_min( p.distance(A), p.distance(B) );
	}
	
	float s = ((Ay-y)*(Bx-Ax)-(Ax-x)*(By-Ay)) / pow2(L);
	return fabs(s)*L;
}

// Collinear points are points that all lie on the same line. 

bool CL_LineMath::collinear( float *lineA, float *lineB )
{
	const float &Ax = lineA[0];
	const float &Ay = lineA[1];
	const float &Bx = lineA[2];
	const float &By = lineA[3];

	const float &Cx = lineB[0];
	const float &Cy = lineB[1];
	const float &Dx = lineB[2];
	const float &Dy = lineB[3];
	
	float denominator = ((Bx-Ax)*(Dy-Cy)-(By-Ay)*(Dx-Cx));	
	float numerator = ((Ay-Cy)*(Dx-Cx)-(Ax-Cx)*(Dy-Cy));

	if( denominator == 0 && numerator == 0 )
		return true;

	return false;
}

bool CL_LineMath::point_right_of_line( float x, float y, float *line )
{
	const float &Ax = line[0];
	const float &Ay = line[1];
	const float &Bx = line[2];
	const float &By = line[3];

	float det = Ax*By - Ax*y + Ay*x - Ay*Bx + Bx*y - By*x;
	return det < 0;
}

bool CL_LineMath::point_right_of_line(float x, float y, float Ax, float Ay, float Bx, float By)
{
	float det = Ax*By - Ax*y + Ay*x - Ay*Bx + Bx*y - By*x;
	return det < 0;
}

/*	    From comp.graphics.algorithms FAQ	 
	+---------------------------------------+

    Let A,B,C,D be 2-space position vectors.  Then the directed line
    segments AB & CD are given by:

        AB=A+r(B-A), r in [0,1]
        CD=C+s(D-C), s in [0,1]

    If AB & CD intersect, then

        A+r(B-A)=C+s(D-C), or

        Ax+r(Bx-Ax)=Cx+s(Dx-Cx)
        Ay+r(By-Ay)=Cy+s(Dy-Cy)  for some r,s in [0,1]

    Solving the above for r and s yields

            (Ay-Cy)(Dx-Cx)-(Ax-Cx)(Dy-Cy)
        r = -----------------------------  (eqn 1)
            (Bx-Ax)(Dy-Cy)-(By-Ay)(Dx-Cx)

            (Ay-Cy)(Bx-Ax)-(Ax-Cx)(By-Ay)
        s = -----------------------------  (eqn 2)
            (Bx-Ax)(Dy-Cy)-(By-Ay)(Dx-Cx)

    Let P be the position vector of the intersection point, then

        P=A+r(B-A) or

        Px=Ax+r(Bx-Ax)
        Py=Ay+r(By-Ay)
*/

CL_Pointf CL_LineMath::get_intersection( float *lineA, float *lineB )
{
	const float &Ax = lineA[0];
	const float &Ay = lineA[1];
	const float &Bx = lineA[2];
	const float &By = lineA[3];

	const float &Cx = lineB[0];
	const float &Cy = lineB[1];
	const float &Dx = lineB[2];
	const float &Dy = lineB[3];

	float denominator = ((Bx-Ax)*(Dy-Cy)-(By-Ay)*(Dx-Cx));

	if( denominator == 0 )
		return CL_Pointf(Ax,Ay); 
	
	float r = ((Ay-Cy)*(Dx-Cx)-(Ax-Cx)*(Dy-Cy)) / denominator;
	
	CL_Pointf P;
	P.x=Ax+r*(Bx-Ax);
	P.y=Ay+r*(By-Ay);
	
	return P;
}

/*
	---- from comp.graphics.algorithms FAQ ----

    By examining the values of r & s, you can also determine some
    other limiting conditions:

        If 0<=r<=1 & 0<=s<=1, intersection exists
            r<0 or r>1 or s<0 or s>1 line segments do not intersect

        If the denominator in eqn 1 is zero, AB & CD are parallel
        If the numerator in eqn 1 is also zero, AB & CD are collinear.
*/

bool CL_LineMath::intersects( 
	float *lineA,
	float *lineB,
	bool collinear_intersect )
{
	const float &Ax = lineA[0];
	const float &Ay = lineA[1];
	const float &Bx = lineA[2];
	const float &By = lineA[3];

	const float &Cx = lineB[0];
	const float &Cy = lineB[1];
	const float &Dx = lineB[2];
	const float &Dy = lineB[3];

	float denominator = ((Bx-Ax)*(Dy-Cy)-(By-Ay)*(Dx-Cx));	
	
	if( denominator == 0 ) // parallell
	{
		if( (Ay-Cy)*(Dx-Cx)-(Ax-Cx)*(Dy-Cy) == 0 ) // collinear
		{
			if( collinear_intersect )
				return true;
			else
				return false; 
		}
		return false;
	}
	
	float r = ((Ay-Cy)*(Dx-Cx)-(Ax-Cx)*(Dy-Cy)) / denominator;
	float s = ((Ay-Cy)*(Bx-Ax)-(Ax-Cx)*(By-Ay)) / denominator;
	
	if( (s >= 0.0f && s <= 1.0f) && (r >= 0.0f && r <= 1.0f) )
		return true;
	
	return false;
}


// return the midpoint on the line from A to B
CL_Pointf CL_LineMath::midpoint( const CL_Pointf &A, const CL_Pointf &B )
{
	return CL_Pointf( (A.x+B.x)/2.0f, (A.y+B.y)/2.0f );
}
