#include "StdAfx.h"
#include <gevec2d.h>
#include "commonRoad.h"
#include "RoadArith.h"


bool CRoadArith::PointInLineDir(const AcGeLineSeg2d &line, bool bDir, const AcGePoint2d &ptInter)
{
	AcGeVector2d lineVec = bDir ? line.startPoint() - line.endPoint() : line.endPoint() - line.startPoint();
	AcGeVector2d interVec = bDir ? ptInter - line.endPoint() : ptInter - line.startPoint();

	if (interVec.dotProduct(lineVec) > 0)
		return true;
	return false;
}

bool CRoadArith::PointInArcDir(const AcGeCircArc2d &arc, bool bDir, const AcGePoint2d &ptInter)
{
	// 延伸长度不超过1/4
	double dExend = 0.25;
	AcGePoint2d accurateStatPoint = bDir ? arc.endPoint() : arc.startPoint();
	AcGePoint2d accurateEndPoint = bDir ? arc.startPoint() : arc.endPoint();
	AcGePoint2d ptCenter = arc.center();

	double dAccurateStartAngle = AcGeVector2d(accurateStatPoint - ptCenter).angle();
	double dAccurateEndAngle = AcGeVector2d(accurateEndPoint - ptCenter).angle();
	double dptAngle = AcGeVector2d(ptInter - ptCenter).angle();

	bool bClock = (arc.isClockWise() == Adesk::kTrue) ? !bDir : bDir;

	// 当前点和accurateStatPoint构成的圆弧角度
	double dInterEndAngle = AngleInClock(bClock, dAccurateStartAngle, dAccurateEndAngle);
	// 圆弧的角度
	double dInterPtAngle = AngleInClock(bClock, dAccurateStartAngle, dptAngle);

	// 圆弧延伸不能太长
	return (dInterPtAngle < dInterEndAngle * (1.0+dExend));
}


void CRoadArith::NearPtInLine(const AcGeLineSeg2d &line, bool bStart, const AcGePoint2d &pt1, const AcGePoint2d &pt2,
						 double &dLen1, double &dLen2)
{
	AcGePoint2d ptAccurate = bStart ? line.startPoint() : line.endPoint();

	dLen1 = pt1.distanceTo(ptAccurate);
	dLen2 = pt2.distanceTo(ptAccurate);
}
void CRoadArith::NearPtInArc(const AcGeCircArc2d &arc, bool bStart, const AcGePoint2d &pt1, const AcGePoint2d &pt2,
						double &dLen1, double &dLen2)
{
	ASSERT(arc.radius() > 1.0);

	// 处于节点区间中的那个点
	AcGePoint2d ptAccurateEnd = bStart ? arc.startPoint() : arc.endPoint();
	AcGePoint2d ptAccurateStart = bStart ? arc.endPoint() : arc.startPoint();
	AcGePoint2d ptCenter = arc.center();
	double dAccurateEndAngle = AcGeVector2d(ptAccurateEnd - ptCenter).angle();
	double dAccurateStartAngle = AcGeVector2d(ptAccurateStart - ptCenter).angle();
	double dAngle1 = AcGeVector2d(pt1 - ptCenter).angle();
	double dAngle2 = AcGeVector2d(pt2 - ptCenter).angle();

	// 角度的计算方向：ptAccurateStart -> ptAccurateEnd;
	bool bClock = (arc.isClockWise() == Adesk::kTrue) ? !bStart : bStart;

	double dInterAngleAccurateEnd = AngleInClock(bClock, dAccurateStartAngle, dAccurateEndAngle);
	double dInterAngle1 = AngleInClock(bClock, dAccurateStartAngle, dAngle1);
	double dInterAngle2 = AngleInClock(bClock, dAccurateStartAngle, dAngle2);

	dLen1 = fabs(dInterAngle1 - dInterAngleAccurateEnd)*arc.radius()*0.5/PI;
	dLen2 = fabs(dInterAngle2 - dInterAngleAccurateEnd)*arc.radius()*0.5/PI;
}




// 直线和圆弧交与两点，找出一个合适的交点
bool CRoadArith::TestProperPoint(const AcGeLineSeg2d &line1, bool bStart1, const AcGeCircArc2d &arc2, bool bStart2,
							const AcGePoint2d &pt1, const AcGePoint2d &pt2, AcGePoint2d &ptDest)
{
	// 按照圆弧的顺序
	double dLen11 = 0.0, dLen12 = 0.0, dLen21 = 0.0, dLen22 = 0.0;
	NearPtInLine(line1, bStart1, pt1, pt2, dLen11, dLen12);
	NearPtInArc(arc2, bStart2, pt1, pt2, dLen21, dLen22);

	if (dLen11 + dLen21 < dLen12 + dLen22)
	{
		ptDest = pt1;
	}
	else
	{
		ptDest = pt2;
	}

	// 交点在线段的方向上，则正确。
	return PointInLineDir(line1, bStart1, ptDest) && PointInArcDir(arc2, bStart2, ptDest);
}

// 两个圆弧有两个交点
bool CRoadArith::TestProperPoint(const AcGeCircArc2d &arc1, bool bStart1, const AcGeCircArc2d &arc2, bool bStart2,
										  const AcGePoint2d &pt1, const AcGePoint2d &pt2, AcGePoint2d &ptDest)
{
	double dLen11 = 0.0, dLen12 = 0.0, dLen21 = 0.0, dLen22 = 0.0;
	NearPtInArc(arc1, bStart1, pt1, pt2, dLen11, dLen12);
	NearPtInArc(arc2, bStart2, pt1, pt2, dLen21, dLen22);

	if (dLen11 + dLen21 < dLen12 + dLen22)
	{
		ptDest = pt1;
	}
	else
	{
		ptDest = pt2;
	}

	// 交点在线段的方向上，则正确。
	return PointInArcDir(arc1, bStart1, ptDest) && PointInArcDir(arc2, bStart2, ptDest);
}



/*
边线倒角算法：
总体描述倒角算法即：先找到倒角的圆心。以该圆心出发向两条边线分别作垂线段，两个交点即为倒角圆弧的两个端点。
查找倒角圆心从数学上描述为：找到一个到两条边线垂直距离皆为倒角半径的一个点，
进一步的描述是：对于每一个边线来看，到其距离为倒角半径的点分别是一个集合，从从这两个集合中找到一个交集，即为倒角圆心
对于线段来讲，这个集合是线段两侧的两个线段，对于圆弧来讲，这个集合是两个圆弧。
因为找圆心的过程既是：找线段与线段，线段与圆弧，或者圆弧与圆弧的交点的过程。

找到圆心之后，下边的操作即简单了。对应的找到垂足即可，而圆心到这两个垂足的距离应该都等于倒角半径。
*/


const double ANGLE_TOL = 0.02;

bool CRoadArith::Off(const AcGeVector2d &vecL1, const AcGeVector2d &vecR1, const AcGeVector2d &vecL2, const AcGeVector2d &vecR2,
		 const AcGeVector2d &dir1, const AcGeVector2d &dir2, bool &bLeft1, bool &bLeft2)
{
	double dDotL1 = vecL1.dotProduct(dir2);
	double dDotR1 = vecR1.dotProduct(dir2);
	double dDotL2 = vecL2.dotProduct(dir1);
	double dDotR2 = vecR2.dotProduct(dir1);

	const double ZERO = 0.0;
	// 说明两个边线平行了。
	if (DoubleEqual(dDotL1, ZERO) || DoubleEqual(dDotL2, ZERO) || DoubleEqual(dDotR1, ZERO) || DoubleEqual(dDotR2, ZERO))
		return false;

	if (dDotL1 > ZERO)
		bLeft1 = true;
	else if (dDotR1 > ZERO)
		bLeft1 = false;
	else
		return false;

	if (dDotL2 > ZERO)
		bLeft2 = true;
	else if (dDotR2 > ZERO)
		bLeft2 = false;
	else
		return false;

	return true;		
}


bool CRoadArith::FindUnitVec(const AcGeLineSeg2d &line1, bool bDir1, const AcGeLineSeg2d &line2, bool bDir2,
				 bool &bLeft1, bool &bLeft2)
{
	AcGeVector2d dir1 = line1.direction();
	AcGeVector2d dir2 = line2.direction();

	// 线段的四个左右向量
	AcGeVector2d vecL1 = dir1.perpVector();
	AcGeVector2d vecR1 = -vecL1;
	AcGeVector2d vecL2 = dir2.perpVector();
	AcGeVector2d vecR2 = -vecL2;

	if (!bDir1)
		dir1 = -dir1;
	if (!bDir2)
		dir2 = -dir2;

	double dAngle1 = dir1.angle();
	double dAngle2 = dir2.angle();

	if (DoubleEqual(dAngle1, dAngle2, ANGLE_TOL))
		return false;
	else if (DoubleEqual(fabs(dAngle1 - dAngle2), PI, ANGLE_TOL))
		return false;

	return Off(vecL1, vecR1, vecL2, vecR2, dir1, dir2, bLeft1, bLeft2);
}


bool CRoadArith::FindUnitVec(const AcGeLineSeg2d &line1, bool bDir1, const AcGeCircArc2d &arc2, bool bDir2,
				 bool &bLeft1, bool &bIn2)
{
	AcGeVector2d dir1 = line1.direction();
	AcGeVector2d dir2 = bDir2 ? TanVecOfArcStartPoint(arc2) : TanVecOfArcEndPoint(arc2);

	AcGeVector2d vecL1 = dir1.perpVector();
	AcGeVector2d vecR1 = -vecL1;
	AcGeVector2d vecIn2 = arc2.center() -(bDir2 ? arc2.startPoint() : arc2.endPoint());
	AcGeVector2d vecOut2 = -vecIn2;

	if (!bDir1)
		dir1 = -dir1;

	double dAngle1 = dir1.angle();
	double dAngle2 = dir2.angle();

	// 180°相切，不倒角
	if (DoubleEqual(fabs(dAngle1-dAngle2), PI, ANGLE_TOL))
		return false;
	else if (DoubleEqual(dAngle1, dAngle2, ANGLE_TOL))
	{ // 这个时候，直线向着圆弧圆心的方向偏，圆弧向外偏
		bIn2 = false; // 向外偏
		AcGeVector2d vecLineToArc = arc2.center() - (bDir1 ? (line1.startPoint()) : (line1.endPoint()));
		if (vecL1.dotProduct(vecLineToArc) > 0) bLeft1 = true; // 左边和圆弧同向
		else bLeft1 = false;

		return true;
	}

	return Off(vecL1, vecR1, vecIn2, vecOut2, dir1, dir2, bLeft1, bIn2);
}

#define ArcStartInAntiClock(arc, bStart) \
	((arc).isClockWise() ? (!bStart) : (bStart))

bool CRoadArith::FindUnitVec(const AcGeCircArc2d &arc1, bool bDir1, const AcGeCircArc2d &arc2, bool bDir2,
				 bool &bIn1, bool &bIn2)
{
	AcGeVector2d dir1 = bDir1 ? TanVecOfArcStartPoint(arc1) : TanVecOfArcEndPoint(arc1);
	AcGeVector2d dir2 = bDir2 ? TanVecOfArcStartPoint(arc2) : TanVecOfArcEndPoint(arc2);

	// 两个圆弧相切：
	// 外切，内切，
	// 外切的时候，两个弧都想外偏，
	// 内切的时候，大圆弧向里偏，小圆弧向外偏
	double dAngle1 = dir1.angle();
	double dAngle2 = dir2.angle();

	if (DoubleEqual(fabs(dAngle1 - dAngle2), PI, ANGLE_TOL))
		return false;
	else if (DoubleEqual(dAngle1, dAngle2, ANGLE_TOL))
	{
		bool bStart1 = ArcStartInAntiClock(arc1, bDir1);
		bool bStart2 = ArcStartInAntiClock(arc2, bDir2);

		if ((!bStart1 && bStart2) || (bStart1 && !bStart2))
		{
			bIn1 = false;
			bIn2 = false;
		}
		else
		{
			if (DoubleEqual(arc1.radius(), arc2.radius()))
				return false; // 半径相等；
			else if (arc1.radius() > arc2.radius())
			{
				bIn1 = true;
				bIn2 = false;
			}
			else
			{
				bIn1 = false;
				bIn2 = true;
			}
		}

		return true;
	}

	AcGeVector2d vecIn1 = arc1.center() -(bDir1 ? arc1.startPoint() : arc1.endPoint());
	AcGeVector2d vecOut1 = -vecIn1;
	AcGeVector2d vecIn2 = arc2.center() -(bDir2 ? arc2.startPoint() : arc2.endPoint());
	AcGeVector2d vecOut2 = -vecIn2;

	return Off(vecIn1, vecOut1, vecIn2, vecOut2, dir1, dir2, bIn1, bIn2);
}



AcGeRay2d CRoadArith::LineSegToRay(const AcGeLineSeg2d &line, bool bDir)
{
	if (bDir)
		return AcGeRay2d(line.endPoint(), line.startPoint());
	else
		return AcGeRay2d(line.startPoint(), line.endPoint());
}

void CRoadArith::ConInterCircle(const AcGePoint2d &ptCenter, const AcGePoint2d &pt1, const AcGePoint2d &pt2, AcGeCircArc2d &interArc)
{
	AcGePoint2d ptMidChord = AcGeLineSeg2d(pt1, pt2).midPoint();
	AcGeVector2d vecMid(ptMidChord - ptCenter);

	double dRadius = ptCenter.distanceTo(pt1);
	AcGePoint2d ptMidArc = vecMid.normalize() * dRadius + ptCenter;

	interArc.set(pt1, ptMidArc, pt2);
}


bool CRoadArith::ForCenterLineAndLine(AcGeCurve2d *pEnt1, bool bDir1, 
									  AcGeCurve2d *pEnt2, bool bDir2, 
									  double dRadius, AcGePoint2d &ptCenter)
{
	AcGeLineSeg2d &line1 = *((AcGeLineSeg2d*)pEnt1);
	AcGeLineSeg2d &line2 = *((AcGeLineSeg2d*)pEnt2);

	if (dRadius < 1.0)
		return false;
	// 平行的不倒角
	if (line1.isParallelTo(line2))
		return false;

	bool bLeft1 = false, bLeft2 = false;
	// 偏移的线段
	if (!FindUnitVec(line1, bDir1, line2, bDir2, bLeft1, bLeft2))
		return false;

	AcGeLineSeg2d lineOff1 = bLeft1 ? LeftLine(line1, dRadius) : RightLine(line1, dRadius);
	AcGeLineSeg2d lineOff2 = bLeft2 ? LeftLine(line2, dRadius) : RightLine(line2, dRadius);

	// 偏移的射线，保证交点的方向正确。
	AcGeRay2d rayOff1 = LineSegToRay(lineOff1, bDir1);
	AcGeRay2d rayOff2 = LineSegToRay(lineOff2, bDir2);

	// 无焦点，说明线段找不到圆心
	if (!rayOff1.intersectWith(rayOff2, ptCenter))
		return false;

	return true;
}

bool CRoadArith::ForCenterLineAndArc(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2, double dRadius, AcGePoint2d &ptCenter)
{
	AcGeLineSeg2d &line1 = *((AcGeLineSeg2d*)pEnt1);
	AcGeCircArc2d &arc2 = *((AcGeCircArc2d*)pEnt2);

	if (dRadius < 1.0)
		return false;

	// 偏移的线段
	bool bLeft1 = false, bIn2 = false;
	if (!FindUnitVec(line1, bDir1, arc2, bDir2, bLeft1, bIn2))
		return false;

	AcGeLineSeg2d lineOff1 = bLeft1 ? LeftLine(line1, dRadius) : RightLine(line1, dRadius);
	AcGeCircArc2d arcOff2;
	if (bIn2)
	{
		if(arc2.radius() < dRadius + 1.0)
			return false;
		arcOff2 = InSideArc(arc2, dRadius);
	}
	else
		arcOff2 = OutSideArc(arc2, dRadius);

	// 偏移的射线
	AcGeRay2d rayOff1 = LineSegToRay(lineOff1, bDir1);
	AcGeCircArc2d circleOff2(arcOff2.center(), arcOff2.radius());

	int nNum = 0;// 交点个数
	AcGePoint2d interPt1, interPt2;
	if (!circleOff2.intersectWith(rayOff1, nNum, interPt1, interPt2))
		return false;

	if (nNum == 1)
		ptCenter = interPt1;
	else 
	{
		if (!TestProperPoint(line1, bDir1, arc2, bDir2, interPt1, interPt2, ptCenter))
			return false;
	}

	return true;
}

bool CRoadArith::ForCenterArcAndLine(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2, double dRadius, AcGePoint2d &ptCenter)
{
	return ForCenterLineAndArc(pEnt2, bDir2, pEnt1, bDir1, dRadius, ptCenter);
}


bool CRoadArith::ForCenterArcAndArc(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2, double dRadius, AcGePoint2d &ptCenter)
{
	AcGeCircArc2d &arc1 = *((AcGeCircArc2d*)pEnt1);
	AcGeCircArc2d &arc2 = *((AcGeCircArc2d*)pEnt2);

	if (dRadius < 1.0)
		return false;

	// 偏移的线段
	bool bIn1 =false, bIn2 = false;
	if (!FindUnitVec(arc1, bDir1, arc2, bDir2, bIn1, bIn2))
		return false;

	// 向着对方移动，然后根据它们求交点
	AcGeCircArc2d arcOff1, arcOff2;
	if (bIn1)
	{
		if (arc1.radius() < dRadius + 1.0)
			return false;
		arcOff1 = InSideArc(arc1, dRadius);
	}
	else
		arcOff1 = OutSideArc(arc1, dRadius);

	if (bIn2)
	{
		if (arc2.radius() < dRadius + 1.0)
			return false;

		arcOff2 = InSideArc(arc2, dRadius);
	}
	else
		arcOff2 = OutSideArc(arc2, dRadius);


	// 偏移的射线
	AcGeCircArc2d circleOff1(arcOff1.center(), arcOff1.radius());
	AcGeCircArc2d circleOff2(arcOff2.center(), arcOff2.radius());

	// 无焦点，说明线段找不到圆心
	int nNum = 0;// 交点个数
	AcGePoint2d interPt1, interPt2;
	if (!circleOff2.intersectWith(circleOff1, nNum, interPt1, interPt2))
		return false;

	if (nNum == 1)
		ptCenter = interPt1;
	else 
	{
		if (!TestProperPoint(arc1, bDir1, arc2, bDir2, interPt1, interPt2, ptCenter))
			return false;
	}

	return true;
}


// 线段跟线段的倒角处理
bool CRoadArith::InterCircleLineAndLine(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2,
										double dRadius, AcGeCircArc2d &interCircle)
{
	AcGeLineSeg2d &line1 = *((AcGeLineSeg2d*)pEnt1);
	AcGeLineSeg2d &line2 = *((AcGeLineSeg2d*)pEnt2);

	AcGePoint2d ptCenter;
	if (!ForCenterLineAndLine(pEnt1, bDir1, pEnt2, bDir2, dRadius, ptCenter))
		return false;

	// 找垂足：
	AcGeRay2d ray1 = LineSegToRay(line1, bDir1);
	AcGeRay2d ray2 = LineSegToRay(line2, bDir2);
	AcGePoint2d perpPt1 = ray1.closestPointTo(ptCenter);
	AcGePoint2d perpPt2 = ray2.closestPointTo(ptCenter);

	// 检测拆分点在直线上是否合适
	if (!PointInLineDir(line1, bDir1, perpPt1) || !PointInLineDir(line2, bDir2, perpPt2))
		return false;

	// 构造圆弧，设置原始线段
	ConInterCircle(ptCenter, perpPt1, perpPt2, interCircle);
	ResetLine(line1, bDir1, perpPt1);
	ResetLine(line2, bDir2, perpPt2);

	return true;	
}


// 线段跟弧线的倒角，参数同上
bool CRoadArith::InterCircleLineAndArc(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2,
									   double dRadius, AcGeCircArc2d &interCircle)
{
	AcGeLineSeg2d &line1 = *((AcGeLineSeg2d*)pEnt1);
	AcGeCircArc2d &arc2 = *((AcGeCircArc2d*)pEnt2);

	AcGePoint2d ptCenter;
	if (!ForCenterLineAndArc(pEnt1, bDir1, pEnt2, bDir2, dRadius, ptCenter))
		return false;

	// 找垂足：
	AcGeRay2d ray1 = LineSegToRay(line1, bDir1);
	AcGeCircArc2d circle2(arc2.center(), arc2.radius());
	AcGePoint2d perpPt1 = ray1.closestPointTo(ptCenter);
	AcGePoint2d perpPt2 = circle2.closestPointTo(ptCenter);

	// 检测拆分点在直线上是否合适
	if (!PointInLineDir(line1, bDir1, perpPt1) || !PointInArcDir(arc2, bDir2, perpPt2))
		return false;

	// 构造圆弧，设置原始线段
	ConInterCircle(ptCenter, perpPt1, perpPt2, interCircle);
	ResetLine(line1, bDir1, perpPt1);
	ResetArc(arc2, bDir2, perpPt2);

	return true;	
}


bool CRoadArith::InterCircleArcAndLine(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2,
									   double dRadius, AcGeCircArc2d &interCircle)
{
	return InterCircleLineAndArc(pEnt2, bDir2, pEnt1, bDir1, dRadius, interCircle);
}




// 弧线跟弧线的倒角，参数同上
bool CRoadArith::InterCircleArcAndArc(AcGeCurve2d *pEnt1, bool bDir1, AcGeCurve2d *pEnt2, bool bDir2,
									  double dRadius, AcGeCircArc2d &interCircle)
{
	AcGeCircArc2d &arc1 = *((AcGeCircArc2d*)pEnt1);
	AcGeCircArc2d &arc2 = *((AcGeCircArc2d*)pEnt2);

	AcGePoint2d ptCenter;
	if (!ForCenterArcAndArc(pEnt1, bDir1, pEnt2, bDir2, dRadius, ptCenter))
		return false;

	// 找垂足：
	AcGeCircArc2d circle1(arc1.center(), arc1.radius());
	AcGeCircArc2d circle2(arc2.center(), arc2.radius());
	AcGePoint2d perpPt1 = circle1.closestPointTo(ptCenter);
	AcGePoint2d perpPt2 = circle2.closestPointTo(ptCenter);

	// 检测拆分点在直线上是否合适
	if (!PointInArcDir(arc1, bDir1, perpPt1) || !PointInArcDir(arc2, bDir2, perpPt2))
		return false;

	// 构造圆弧，设置原始线段
	ConInterCircle(ptCenter, perpPt1, perpPt2, interCircle);
	ResetArc(arc1, bDir1, perpPt1);
	ResetArc(arc2, bDir2, perpPt2);

	return true;	
}