//-----------------------------------------------------------------------------------------------
// GeometricQueries2D.cpp
//
#include "GypsyPrecompiledHeader.hpp"

//-----------------------------------------------------------------------------------------------
#include "Engine/Core/GeometricQueries2D.hpp"


//-----------------------------------------------------------------------------------------------
float CalcDistanceSquaredPointToLineSegment2D( const Vector2& point, const Vector2& lineSegmentStart, const Vector2& lineSegmentEnd )
{
	const float lineDisplacementX = lineSegmentEnd.x - lineSegmentStart.x;
	const float lineDisplacementY = lineSegmentEnd.y - lineSegmentStart.y;
	const float startToPointX = point.x - lineSegmentStart.x;
	const float startToPointY = point.y - lineSegmentStart.y;
	const float dotWithDisplacement = (lineDisplacementX * startToPointX) + (lineDisplacementY * startToPointY);
	if( dotWithDisplacement <= 0.f )
	{
		return (startToPointX * startToPointX) + (startToPointY * startToPointY);
	}

	const float displacementLengthSquared = (lineDisplacementX * lineDisplacementX) + (lineDisplacementY * lineDisplacementY);
	if( dotWithDisplacement >= displacementLengthSquared )
	{
		const float endToPointX = point.x - lineSegmentEnd.x;
		const float endToPointY = point.y - lineSegmentEnd.y;
		return (endToPointX * endToPointX) + (endToPointY * endToPointY);
	}
	else
	{
		const float dotWithPerpendicularDisplacement = (lineDisplacementY * startToPointX) - (lineDisplacementX * startToPointY);
		const float scale = dotWithPerpendicularDisplacement / displacementLengthSquared;
		const float perpendicularDistanceX = scale * lineDisplacementY;
		const float perpendicularDistanceY = scale * lineDisplacementX;
		return (perpendicularDistanceX * perpendicularDistanceX) + (perpendicularDistanceY * perpendicularDistanceY);
	}
}


//-----------------------------------------------------------------------------------------------
const Vector2 FindClosestPointOnLineSegment2D( const Vector2& point, const Vector2& lineSegmentStart, const Vector2& lineSegmentEnd )
{
	const float lineDisplacementX = lineSegmentEnd.x - lineSegmentStart.x;
	const float lineDisplacementY = lineSegmentEnd.y - lineSegmentStart.y;
	const float startToPointX = point.x - lineSegmentStart.x;
	const float startToPointY = point.y - lineSegmentStart.y;
	const float dotWithDisplacement = (lineDisplacementX * startToPointX) + (lineDisplacementY * startToPointY);
	if( dotWithDisplacement <= 0.f )
	{
		return lineSegmentStart;
	}

	const float displacementLengthSquared = (lineDisplacementX * lineDisplacementX) + (lineDisplacementY * lineDisplacementY);
	if( dotWithDisplacement >= displacementLengthSquared )
	{
		return lineSegmentEnd;
	}
	else
	{
		const float scale = dotWithDisplacement / displacementLengthSquared;
		const float projectionDisplacementX = scale * lineDisplacementX;
		const float projectionDisplacementY = scale * lineDisplacementY;
		return Vector2( lineSegmentStart.x + projectionDisplacementX, lineSegmentStart.y + projectionDisplacementY );
	}
}


//-----------------------------------------------------------------------------------------------
float CalcDistanceSquaredPointToLine2D( const Vector2& point, const Vector2& firstPointOnLine, const Vector2& secondPointOnLine )
{
	const float lineDisplacementX = secondPointOnLine.x - firstPointOnLine.x;
	const float lineDisplacementY = secondPointOnLine.y - firstPointOnLine.y;
	const float firstToPointX = point.x - firstPointOnLine.x;
	const float firstToPointY = point.y - firstPointOnLine.y;
	const float dotWithDisplacement = (lineDisplacementX * firstToPointX) + (lineDisplacementY * firstToPointY);
	const float displacementLengthSquared = (lineDisplacementX * lineDisplacementX) + (lineDisplacementY * lineDisplacementY);
	if( displacementLengthSquared > 0.f )
	{
		const float scale = dotWithDisplacement / displacementLengthSquared;
		const float perpendicularDistanceX = scale * lineDisplacementY;
		const float perpendicularDistanceY = scale * lineDisplacementX;
		return (perpendicularDistanceX * perpendicularDistanceX) + (perpendicularDistanceY * perpendicularDistanceY);
	}
	else
	{
		// Degenerate case: line is collapsed onto itself (start == end), so we cannot infer line direction
		return (firstToPointX * firstToPointX) + (firstToPointY * firstToPointY);
	}
}


//-----------------------------------------------------------------------------------------------
const Vector2 FindClosestPointOnLine2D( const Vector2& point, const Vector2& firstPointOnLine, const Vector2& secondPointOnLine )
{
	const float lineDisplacementX = secondPointOnLine.x - firstPointOnLine.x;
	const float lineDisplacementY = secondPointOnLine.y - firstPointOnLine.y;
	const float firstToPointX = point.x - firstPointOnLine.x;
	const float firstToPointY = point.y - firstPointOnLine.y;
	const float dotWithDisplacement = (lineDisplacementX * firstToPointX) + (lineDisplacementY * firstToPointY);
	const float displacementLengthSquared = (lineDisplacementX * lineDisplacementX) + (lineDisplacementY * lineDisplacementY);
	if( displacementLengthSquared > 0.f )
	{
		const float scale = dotWithDisplacement / displacementLengthSquared;
		const float projectionDisplacementX = scale * lineDisplacementX;
		const float projectionDisplacementY = scale * lineDisplacementY;
		return Vector2( firstPointOnLine.x + projectionDisplacementX, firstPointOnLine.y + projectionDisplacementY );
	}
	else
	{
		// Degenerate case: line is collapsed onto itself (start == end), so we cannot infer line direction
		return firstPointOnLine;
	}
}


