//
//	Copyright © 2005 Nemetschek North America 
//	All Rights Reserved.
//
// Created by Vladislav Stanev

#include "StdAfx.h"
//#include "VWArc.h"

using namespace VWFC::Math;

#ifdef _DEBUG
# define CHECK_INNERDATA_CONSISTENCY		this->CheckInnerDataConsistency()
#else
# define CHECK_INNERDATA_CONSISTENCY
#endif

VWArc2D::VWArc2D()
{
	fRadius			= 0;
	fStartAngleDeg	= 0;
	fSweepAngleDeg	= 0;

}

VWArc2D::VWArc2D(const VWPoint2D& center, double radius)
{
	this->SetArc( center, radius );
}

VWArc2D::VWArc2D(const VWPoint2D& center, double radius, double startAngleDeg, double sweepAngleDeg)
{
	this->SetArc( center, radius, startAngleDeg, sweepAngleDeg );
}

VWArc2D::VWArc2D(const VWPoint2D& startPT, const VWPoint2D& ptOnArc, const VWPoint2D& endPt)
{
	this->SetArc( startPT, ptOnArc, endPt );
}

VWArc2D::VWArc2D(const VWArc2D& src)
{
	fCenterPt				= src.fCenterPt;
	fStartPt			= src.fStartPt;
	fEndPt				= src.fEndPt;

	fRadius				= src.fRadius;
	fStartAngleDeg		= src.fStartAngleDeg;
	fSweepAngleDeg		= src.fSweepAngleDeg;

	CHECK_INNERDATA_CONSISTENCY;
}

VWArc2D::~VWArc2D()
{
}

VWArc2D& VWArc2D::operator=(const VWArc2D& src)
{
	fCenterPt			= src.fCenterPt;
	fStartPt			= src.fStartPt;
	fEndPt				= src.fEndPt;

	fRadius				= src.fRadius;
	fStartAngleDeg		= src.fStartAngleDeg;
	fSweepAngleDeg		= src.fSweepAngleDeg;

	return *this;
}

void VWArc2D::SetArc(const VWPoint2D& center, double radius)
{
	this->SetArc( center, radius, 0, 360 );
	CHECK_INNERDATA_CONSISTENCY;
}

void VWArc2D::SetArc(const VWPoint2D& startPt, const VWPoint2D& ptOnArc, const VWPoint2D& endPt)
{
	fStartPt				= startPt;
	fEndPt					= endPt;
	fCenterPt.ThreePtCenter( fStartPt, ptOnArc, fEndPt );

	VWPoint2D	startDir	= fStartPt - fCenterPt;
	VWPoint2D	endDir		= fEndPt - fCenterPt;
	fRadius					= fCenterPt.DistanceTo( fStartPt );
	VWFC_ASSERT( MathUtils::Equalish( fRadius, fCenterPt.DistanceTo( ptOnArc ), 0.0001 ) );
	VWFC_ASSERT( MathUtils::Equalish( fRadius, fCenterPt.DistanceTo( fEndPt ), 0.0001 ) );

	fStartAngleDeg			= startDir.CalcAng360();
	double		endAngDeg	= endDir.CalcAng360();
	if ( endAngDeg < fStartAngleDeg )
		endAngDeg		+= 360;
	fSweepAngleDeg			= endAngDeg - fStartAngleDeg;

	if ( ! this->IsPtOnArc( ptOnArc, VWPoint2D::sEpsilon ) ) {
		fSweepAngleDeg	-= 360;
	}

	CHECK_INNERDATA_CONSISTENCY;
}

void VWArc2D::SetArc(const VWPoint2D& center, double radius, double startAngleDeg, double sweepAngleDeg)
{
	fCenterPt			= center;

	// ensure proper positive [0;360] start angle
	while ( startAngleDeg < 0 )
		startAngleDeg	+= 360;
	while ( MathUtils::GEish( startAngleDeg, 360, VWPoint2D::sEpsilon ) )
		startAngleDeg	-= 360;
	
	VWPoint2D	a, b;
	a.SetAngle( MathUtils::Deg2Rad( startAngleDeg ) );
	b.SetAngle( MathUtils::Deg2Rad( startAngleDeg + sweepAngleDeg ) );

	fStartPt			= fCenterPt + radius * a;
	fEndPt				= fCenterPt + radius * b;

	fRadius				= radius;
	fStartAngleDeg		= startAngleDeg;
	fSweepAngleDeg		= sweepAngleDeg;

	CHECK_INNERDATA_CONSISTENCY;
}

void VWArc2D::GetArc(VWPoint2D& outCenter, double& outRadius) const
{
	outCenter		= fCenterPt;
	outRadius		= fRadius;
}

void VWArc2D::GetArc(VWPoint2D& outCenter, double& outRadius, double& outStartAngleDeg, double& outSweepAngleDeg) const
{
	outCenter			= fCenterPt;
	outRadius			= fRadius;
	outStartAngleDeg	= fStartAngleDeg;
	outSweepAngleDeg	= fSweepAngleDeg;
}

void VWArc2D::GetArc(VWPoint2D& outStartPt, VWPoint2D& outPtOnArc, VWPoint2D& outEndPt) const
{
	outStartPt			= fStartPt;
	outEndPt			= fEndPt;
	outPtOnArc			= this->GetPtOnArc();
}

VWPoint2D VWArc2D::GetCenterPt() const
{
	return fCenterPt;
}

VWPoint2D VWArc2D::GetStartPt() const
{
	return fStartPt;
}

VWPoint2D VWArc2D::GetEndPt() const
{
	return fEndPt;
}

VWPoint2D VWArc2D::GetPtOnArc() const
{
	VWPoint2D	dir		= fEndPt - fStartPt;
	dir.SetPoint( -dir.y, dir.x );
	dir.Normalize();

	// check if the found point is on the arc
	VWPoint2D	pt = fCenterPt + dir * fRadius;
	if ( this->IsPtOnArc( pt, VWPoint2D::sEpsilon ) )
		return pt;
	else
		return fCenterPt - dir * fRadius;
}

VWPoint2D VWArc2D::GetPtOnArc(double offset) const
{
	double		ang		= offset / fRadius;
	double		cosAng	= cos( ang );
	double		sinAng	= sin( ang );

	// translate the first point into local coords for rotation
	VWPoint2D	locPt	= fStartPt - fCenterPt;

	// rotate first local point
	VWPoint2D	pt;
	pt.x				= locPt.x * cosAng - locPt.y * sinAng;
	pt.y				= locPt.x * sinAng + locPt.y * cosAng;
	
	// transform back to global coords already rotated point
	pt					+= fCenterPt;

	return pt;
}

VWPoint2D VWArc2D::GetPtOnArcAngle(double angDeg)
{
	double		ang		= MathUtils::Deg2Rad( angDeg );
	double		cosAng	= cos( ang );
	double		sinAng	= sin( ang );

	// translate the first point into local coords for rotation
	VWPoint2D	locPt	= fStartPt - fCenterPt;

	// rotate first local point
	VWPoint2D	pt;
	pt.x				= locPt.x * cosAng - locPt.y * sinAng;
	pt.y				= locPt.x * sinAng + locPt.y * cosAng;
	
	// transform back to global coords already rotated point
	pt					+= fCenterPt;

	return pt;
}

double VWArc2D::GetRadius() const
{
	return fRadius;
}

double VWArc2D::GetStartAngle() const
{
	return fStartAngleDeg;
}

double VWArc2D::GetSweepAngle() const
{
	return fSweepAngleDeg;
}

double VWArc2D::GetLength() const
{
	return fRadius * MathUtils::Deg2Rad( fSweepAngleDeg );
}

double VWArc2D::GetLength(const VWPoint2D& pt) const
{
	VWPoint2D	vecPt		= pt - fCenterPt;
	VWPoint2D	vecStart	= fStartPt - fCenterPt;
	double		cosAng		= vecPt.Normalize().DotProduct( vecStart.Normalize() );
	double		ang			= acos( cosAng );

	return fRadius * ang;
}

double VWArc2D::CalcSweepAngle(const VWPoint2D& pt) const
{
	VWPoint2D	vecPt		= pt - fCenterPt;
	VWPoint2D	vecStart	= fStartPt - fCenterPt;
	double		ang			= vecPt.CalcAng360() - vecStart.CalcAng360();

	return ang;
}

VWPoint2D VWArc2D::PtPerpArc(const VWPoint2D& pt) const
{
	VWPoint2D	vecPt		= pt - fCenterPt;
	VWPoint2D	vecStart	= fStartPt - fCenterPt;
	double		cosAng		= vecPt.Normalize().DotProduct( vecStart.Normalize() );
	double		sinAng		= sqrt( 1 - cosAng * cosAng );

	// translate the first point into local coords for rotation
	VWPoint2D	locPt	= fStartPt - fCenterPt;

	// rotate first local point
	VWPoint2D	perpPt;
	perpPt.x			= locPt.x * cosAng - locPt.y * sinAng;
	perpPt.y			= locPt.x * sinAng + locPt.y * cosAng;
	
	// transform back to global coords already rotated point
	perpPt				+= fCenterPt;

	return perpPt;
}

double VWArc2D::Distance(const VWPoint2D& pt) const
{
	VWPoint2D	ptOnArc	= this->PtPerpArc( pt );
	return pt.DistanceTo( ptOnArc );
}

bool VWArc2D::IsPtOnArc(const VWPoint2D& pt, double dEpsilon) const
{
	double		radius			= fCenterPt.DistanceTo( pt );
	if ( ! MathUtils::Equalish( radius, fRadius, dEpsilon ) )
		return false;
    
	VWPoint2D	ptDir		= pt - fCenterPt;
	double		angle		= ptDir.CalcAng360();
	double		ptSweep		= angle - fStartAngleDeg;

	bool	ptOnArc	= false;
	if ( MathUtils::Greaterish( fSweepAngleDeg, 0, dEpsilon ) ) {
		if ( ptSweep < 0 )		ptSweep += 360;
		ptOnArc	= MathUtils::GEish( ptSweep, 0, dEpsilon ) && MathUtils::LEish( ptSweep, fSweepAngleDeg, dEpsilon );
	}
	else {
		ptOnArc	= MathUtils::LEish( ptSweep, 0, dEpsilon ) && MathUtils::GEish( ptSweep, fSweepAngleDeg, dEpsilon );
	}

	return ptOnArc;
}

bool VWArc2D::IsPtOnArc(const VWPoint2D& pt, double dEpsilon, double& outOffset) const
{
	double		radius			= fCenterPt.DistanceTo( pt );
	if ( ! MathUtils::Equalish( radius, fRadius, dEpsilon ) )
		return false;

	VWPoint2D	ptDir		= pt - fCenterPt;
	double		angle		= ptDir.CalcAng360();
	double		ptSweep		= angle - fStartAngleDeg;

	bool	ptOnArc	= false;
	if ( MathUtils::GEish( ptSweep, 0, dEpsilon ) ) {
		ptOnArc	= MathUtils::Greaterish( fSweepAngleDeg, 0, dEpsilon ) && MathUtils::LEish( ptSweep, fSweepAngleDeg, dEpsilon );
	}
	else {
		ptOnArc	= MathUtils::Lesserish( fSweepAngleDeg, 0, dEpsilon ) && MathUtils::GEish( ptSweep, fSweepAngleDeg, dEpsilon );
	}

	if ( ptOnArc ) {
		outOffset = Abs(ptSweep * kRadiansPerDegree * fRadius);
	}

	return ptOnArc;
}

#ifdef _DEBUG
void VWArc2D::CheckInnerDataConsistency()
{
	// CHECK FORWARD
	{
		double		radius			= fCenterPt.DistanceTo( fStartPt );
		VWFC_ASSERT( MathUtils::Equalish( radius, fCenterPt.DistanceTo( fEndPt ), 0.0001 ) );

		VWPoint2D	startDir		= fStartPt - fCenterPt;
		VWPoint2D	endDir			= fEndPt - fCenterPt;

		double		startAngleDeg	= startDir.CalcAng360();
		double		endAngDeg		= endDir.CalcAng360();
		if ( endAngDeg < startAngleDeg )
			endAngDeg		+= 360;
//		double		sweepAngleDeg	= endAngDeg - fStartAngleDeg;


		VWFC_ASSERT( MathUtils::Equalish( radius, fRadius, 0.0001 ) );
		VWFC_ASSERT( MathUtils::Equalish( startAngleDeg, fStartAngleDeg, 0.0001 ) );
		//VWFC_ASSERT( MathUtils::Equalish( sweepAngleDeg, fSweepAngleDeg, 0.0001 ) );
	}

	// CHECK BACKWARD
	{
		VWPoint2D	a, b;
		a.SetAngle( MathUtils::Deg2Rad( fStartAngleDeg ) );
		b.SetAngle( MathUtils::Deg2Rad( fStartAngleDeg + fSweepAngleDeg ) );

		VWPoint2D	startPt		= fCenterPt + fRadius * a;
		VWPoint2D	endPt		= fCenterPt + fRadius * b;

		VWFC_ASSERT( MathUtils::Equalish( startPt, fStartPt, 0.0001 ) );
		VWFC_ASSERT( MathUtils::Equalish( endPt, fEndPt, 0.0001 ) );
	}
}
#endif

/*static*/ void	 VWArc2D::SelfTest()
{

	VWPoint2D	z(0,		0 );
	VWPoint2D	a(-10,		0);
	VWPoint2D	b(0,		-10);
	VWPoint2D	c(7.071,	7.071);
	VWPoint2D	d(-7.071,	-7.071);

	VWArc2D	arc11( z, 10, 180, 90 );	// small arc
	VWArc2D	arc12( z, 10, 270, -90 );

	VWArc2D	arc21( z, 10, 270, 270 );	// big arc
	VWArc2D	arc22( z, 10, 180, -270 );

	VWArc2D	arc31( a, d, b );		// small arc
	VWArc2D	arc32( a, c, b );		// big arc

	ASSERTN( kVStanev, arc11.IsPtOnArc( d, 0.0001 ) );		// small arc
	ASSERTN( kVStanev, arc12.IsPtOnArc( d, 0.0001 ) );
	ASSERTN( kVStanev, arc31.IsPtOnArc( d, 0.0001 ) );

	ASSERTN( kVStanev, ! arc11.IsPtOnArc( c, 0.0001 ) );	// small arc
	ASSERTN( kVStanev, ! arc12.IsPtOnArc( c, 0.0001 ) );
	ASSERTN( kVStanev, ! arc31.IsPtOnArc( c, 0.0001 ) );

	ASSERTN( kVStanev, arc21.IsPtOnArc( c, 0.0001 ) );		// big arc
	ASSERTN( kVStanev, arc22.IsPtOnArc( c, 0.0001 ) );
	ASSERTN( kVStanev, arc32.IsPtOnArc( c, 0.0001 ) );

	ASSERTN( kVStanev, ! arc21.IsPtOnArc( d, 0.0001 ) );	// big arc
	ASSERTN( kVStanev, ! arc22.IsPtOnArc( d, 0.0001 ) );
	ASSERTN( kVStanev, ! arc32.IsPtOnArc( d, 0.0001 ) );
}


