line.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

/*
 *  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();
}