/* *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET). * *  This program is free software, you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation. * *  This program 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 Library General Public License for more details. * *  This file is created by: Mostafa Rokooey *  and is modified by: Sassan Haradji * *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team. *  For more information please read README file.*/#include <cmath>#include <Line.h>#include <HalfLine.h>#include <Circle.h>#include <Degree.h>#include <Vector.h>using namespace std;using namespace Degree;float Line::getA(void){	return a;}float Line::getB(void){	return b;}float Line::getC(void){	return c;}float Line::getDirection(void){	return direction;}unsigned Line::getLineIntersect(Line line, Point &intersectPoint){	float a1 = a, b1 = b, c1 = c;	float a2 = line.getA(), b2 = line.getB(), c2 = line.getC();	intersectPoint.x = ((b1 * c2) - (b2 * c1)) / ((a1 * b2) - (a2 * b1));	intersectPoint.y = ((a2 * c1) - (a1 * c2)) / ((a1 * b2) - (a2 * b1));	return 1;}unsigned Line::getHalfLineIntersect(HalfLine halfLine, Point &intersectPoint){	float a1 = a, b1 = b, c1 = c;	float a2 = halfLine.getA(), b2 = halfLine.getB(), c2 = halfLine.getC();	intersectPoint.x = ((b1 * c2) - (b2 * c1)) / ((a1 * b2) - (a2 * b1));	intersectPoint.y = ((a2 * c1) - (a1 * c2)) / ((a1 * b2) - (a2 * b1));	if (halfLine.checkPoint(intersectPoint))		return 1;	return 0;}unsigned Line::getCircleIntersect(Circle circle, vector <Point> &points){	float x0 = circle.getCenter().x, y0 = circle.getCenter().y;	float r = circle.getRadius();	float formulaA, formulaB, formulaC;	float delta;	Point ansPoint1, ansPoint2;	formulaA = (pow(a, 2) + pow(b, 2)) / pow(a, 2);	formulaB = ((2 * a * b * x0) - (2 * b * c) - (2 * pow(a, 2) * y0)) /		pow(a, 2);	formulaC = (pow(a * x0, 2) + pow(a * y0, 2) - pow(a * r, 2) + pow(c, 2) +		(2 * a * c * x0)) / pow(a, 2);	delta = pow(formulaB, 2) - (4 * formulaA * formulaC);	if (delta < 0)		return 0;	ansPoint1.y = ((0 - formulaB) + sqrt(delta)) / (2 * formulaA);	ansPoint2.y = ((0 - formulaB) - sqrt(delta)) / (2 * formulaA);	ansPoint1.x = (0 - ((b * ansPoint1.y) + c)) / a;	ansPoint2.x = (0 - ((b * ansPoint2.y) + c)) / a;	if (delta == 0)	{		points.push_back(ansPoint1);		return 1;	}		points.push_back(ansPoint1);	points.push_back(ansPoint2);	return 2;	}bool Line::checkPoint(Point point){	if (((a * point.x) + (b * point.y) + c) == 0)		return true;	return false;}void Line::setByABC(float inpA, float inpB, float inpC){	a = inpA;	b = inpB;	c = inpC;	if (b == 0)		direction = 90;	else if (a == 0)		direction = 0;	else		direction = arcTan2(-c / b, -c / a);}void Line::setBySourceDir(Point point, float dir){	float cs, sn, n;	if (((dir + 90) / 180) == (int)((dir + 90) / 180))	{		b = 0;		a = 1;		c = 0 - point.x;		m = 0xFFFFFF;	}	else	{				m = Degree::tan(dir);		n = point.y - (m * point.x);/*		a = 0 - m;		b = 1;		c = 0 - n;*/		sn = Degree::sin(dir);		cs = Degree::cos(dir);		a = 0 - sn;		b = cs;		c = 0 - (cs * n);	}	direction = dir;}void Line::setByPoints(Point point1, Point point2){//	float cs, sn	float n;	if (point2.x == point1.x)	{		b = 0;		a = 1;		c = 0 - point1.x;		m = 0xFFFFFF;	}	else	{		m = (point2.y - point1.y) / (point2.x - point1.x);		n = point1.y - (point1.x * m);		a = point1.y - point2.y;		b = point2.x - point1.x;		c = point1.y * (point1.x - point2.x) + point1.x * (point2.y - point1.y);/*		a = 0 - sn;		b = cs;		c = 0 - (cs * n);*/	}	direction = Degree::arcTan2(point2.y - point1.y, point2.x - point1.x);}float Line::getPointDist(Point point){	Line tempLine;	Point interPoint;	Vector tempVector;		tempLine.setBySourceDir(point, Degree::normalizeAngle(direction + 90));	getLineIntersect(tempLine, interPoint);	tempVector.setByPoints(interPoint,point);		return tempVector.getMagnitude();}