geometry.cpp

robocup源代码2001年清华机器人源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "Geometry.h"
#include "utils.h"

/*********************************       Ray   ****************************/
bool Ray::OnRay(Vector& pt)
{
  Vector gap = pt - origin;
  return (fabs(Sin(gap.Angle() - angle) * gap.mod()) < FLOAT_EPS)
    ? true : false;
}

AngleDeg Ray::Allowed_AngleDiff(float distance){
	return float(10 - 5*exp(-distance * 0.2f));
}

AngleDeg Ray::Allowed_AngleDiff(Vector point){
	return Allowed_AngleDiff(point.dist(origin));
}

bool Ray::InRightDir(Vector point){
	return fabs(NormalizeAngle((point - origin).Angle() - angle)) < Allowed_AngleDiff(point);
}

bool Ray::InRightDir(AngleDeg ang){
	return fabs(NormalizeAngle(ang - angle)) < Allowed_AngleDiff();
}

bool Ray::InOppositeDir(Vector& point){
	return fabs(NormalizeAngle((point - origin).Angle() - angle + 180)) < Allowed_AngleDiff(point);
}

bool Ray::InOppositeDir(AngleDeg ang){
	return fabs(NormalizeAngle(ang - angle + 180)) < Allowed_AngleDiff();
}

bool Ray::intersection(Line l, Vector &pPt){
  return l.RayIntersection(*this, pPt);
}

bool Ray::intersection(Ray r, Vector &pPt){
	Line thisLine(*this), argLine(r);

	if (thisLine.SameSlope(argLine))
		return false;
	pPt = thisLine.intersection(argLine);

   //now make sure that the intersection is the correct direction on both lines 
	return  InRightDir(pPt) && r.InRightDir(pPt);
}

float Ray::intersection(Line l){
	return (-l.C -l.A * origin.x - l.B * origin.y) / ( l.A* direction.x + l.B * direction.y);
}
/* intersects a ray and a cricle */
/* return the number of solutions */
/* psol1 1 is not as far along the ray as psol2 */
int Ray::CircleIntersect(float radius, Vector center, Vector& psol1, Vector& psol2)
{
	float a,b,c,disc;
	float t1, t2;
	a = Sqr(direction.x) + Sqr(direction.y);
	b = 2.0f * ((origin.x-center.x) * direction.x + (origin.y-center.y) * direction.y);
	c = Sqr(origin.x-center.x) + Sqr(origin.y-center.y) - Sqr(radius);

	disc = Sqr(b) - 4 * a * c;
	if (disc < 0) {
		return 0; // No solutions
	}
  
	disc = (float) sqrt(disc);
	t1 = (-b + disc) / (2.0f * a);
	t2 = (-b - disc) / (2.0f * a);
  
	if (t1 > t2) {
		float temp = t1;
		t1 = t2;
		t2 = temp;
	}
 
	if (t1 > 0.0) {
		if (t2 > 0.0) {
			psol1 = origin + direction * t1;
			psol2 = origin + direction * t2;
			return 2; // Two solutions
		}
		else{
			my_error("RayCircleIntersect: weird roots");
			return 0;
		}
	}
	else if (t2 > 0.0) {
		psol1 = origin + direction * t2;
		psol2 = psol1;
		return 1;
	}
	else
		return 0;
	return 0;
}

Vector Ray::GetClosestPoint(Vector& point){
  Line l(*this);
  Vector close_point = l.ProjectPoint(point);
  if (OnRay(close_point))
    return close_point;
  else
    return origin;
}

AngleDeg Ray::StrechAngle(Vector& point){
	return NormalizeAngle((float)fabs((point - origin).Angle() - angle));
}

float Ray::GetClosestPointParam(Vector& point){
	return DistanceFromOrigin(point) * Cos(StrechAngle(point));
}

float Ray::DistanceFromOrigin(Vector& point){
	return (point -origin).mod();
}

void Ray::Normalize(){
	if (direction.mod2() != 0){
		direction /= direction.mod();
	}
}
/****************************   Line           ***************************/
Line::Line(float A, float B, float C) {
	this->A = A;
	this->B = B;
	this->C = C;
}

/********************************************************************************/

void Line::LineFromTwoPoints(Vector pt1, Vector pt2)
{
	float temp = (pt2.x - pt1.x);
	if (fabs(temp) < FLOAT_EPS) {
		if (fabs(pt2.y - pt1.y) < FLOAT_EPS)
			my_error("LineFromTwoPoints: points can not be the same!");
		A = 1;
		B = 0;
	}
	else {
		float m = (pt2.y - pt1.y) / temp;
		A = -m;
		B = 1;
	}  
	C = -(A * pt2.x + B * pt2.y);
}

/********************************************************************************/

void Line::LineFromRay(Ray r)   
{
	if (fabs(r.direction.y) < FLOAT_EPS && fabs(r.direction.x) < FLOAT_EPS)
		my_error("LineFromRay: dir can not be zero");
	LineFromTwoPoints(r.origin, r.origin + r.direction);
}

/********************************************************************************/

bool Line::PointOnLine(float x, float y){
  return (bool)(fabs(A * x + B * y + C) < FLOAT_EPS);
}

/********************************************************************************/

float Line::dist(Vector point){
	return (float)fabs( (A*point.x + B*point.y + C) / sqrt(Sqr(A) + Sqr(B)) );
}


float Line::dist2(Vector point)     {
	return (float) fabs( Sqr(A*point.x + B*point.y + C) / (Sqr(A) + Sqr(B)) );
}


float Line::angle(){
	return ATan(-A / B);
}

/********************************************************************************/

float Line::get_y(float x){
	if ( B != 0 ) 
		return (-A*x - C)/B;
	my_error("can't get y"); 
	return 0;
}

float Line::get_x(float y){
	if ( A != 0 ) 
		return (-B*y - C)/A;
	my_error("can't get x"); 
	return 0;
}

/********************************************************************************/

bool Line::InBetween(Vector point, Vector end1, Vector end2){
	if (!OnLine(end1) || !OnLine(end2))
		my_error("Line::InBetween: passed in points that weren't on line");

	point = ProjectPoint(point);
	float dist2 = end1.dist2(end2);
  
	return point.dist2(end1) <= dist2 && point.dist2(end2) <= dist2;
}

/********************************************************************************/

Vector Line::GetClosestPtInBetween(Vector pt, Vector end1, Vector end2){
	if (!OnLine(end1) || !OnLine(end2))
		my_error("Line::InBetween: passed in points that weren't on line");

	if (InBetween(pt, end1, end2))
		return ProjectPoint(pt);
	if (end1.dist2(pt) < end2.dist2(pt))
		return end1;
	else
		return end2;
}


/********************************************************************************/

Vector Line::intersection(Line l){
	Vector result = 0;
	if (SameSlope(l)) {
    //if ( B == 0 && l.B == 0 || A/B == l.A/l.B ){
		my_error("Lines have same slope");
		return result;
	}

	if ( B == 0 ){
		result.x = -C/A;
		result.y = l.get_y(result.x);
		return result;
	}

	if ( l.B == 0){
		result.x = -l.C/l.A;
		result.y = get_y(result.x);
		return result;
	}

	result.x = (C*l.B - B*l.C)/(l.A*B - A*l.B);
	result.y = get_y(result.x);
	return result;
}  

/********************************************************************************/
bool Line::RayIntersection(Ray r, Vector &ppt)
{
	Line lRay(r);

	if (SameSlope(lRay))
		return false;
  
	ppt = intersection(lRay);
	return (r.InRightDir(ppt));
}


/********************************************************************************/
bool Line::IsPtCloserToPtOnLine(Vector point1, Vector point2, Vector target_point)
{
	if (!OnLine(target_point)) 
		my_error("IsPtCloserToPtOnLine: target_point not on line");
  
	point1 = ProjectPoint(point1);
	point2 = ProjectPoint(point2);

	return point1.dist(target_point) < point2.dist(target_point);
}


/********************************************************************************/

//return true on top/left part of plane
bool Line::HalfPlaneTest(Vector point)
{
	if (B==0)
		return point.x < -C/A;
	return point.y > get_y(point.x);
}

bool Line::SameSlope(Line l){
	return B == 0 && l.B == 0 || A/B == l.A/l.B;
}

int Line::CircleIntersect(float radius, Vector& center, Vector &psol1, Vector& psol2){
	int numSol;
	psol1 = psol2 = Vector(0,0);
	if (fabs(A) > FLOAT_EPS) {
		float a,b,c;
		a = 1 + Sqr(B/A);
		b = 2*(-center.y + (C/A + center.x) * (B/A));
		c = -Sqr(radius) + Sqr(C/A + center.x) + Sqr(center.y);
		numSol = QuadraticFormula(a, b, c, psol1.y, psol2.y);
		psol1.x = - ( (B * psol1.y + C) / A );
		psol2.x = - ( (B * psol2.y + C) / A );
		return numSol;
	}
	else{
		numSol = QuadraticFormula(1, -2*center.x, Sqr(center.x) + Sqr(C/B + center.y) - Sqr(radius), psol1.x, psol2.x);
		psol1.y = psol2.y = - C / B;
		return numSol;
	}
}

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        Rect Class                                       */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

Rect::Rect()
{
  left_x = right_x = top_y = bottom_y = 0;
}

/********************************************************************************/

Rect::Rect(float l, float r, float t, float b){
	if ( l >= r ) Swap(l, r);
	if ( t >= b ) Swap(t, b);

	left_x = l ; right_x = r ;
	top_y = t ; bottom_y = b ;
}
Rect::Rect(const Vector& center, const Vector& size){
	left_x = (float)(center.x - fabs(size.x)/2.0f);
	right_x = (float)(center.x + fabs(size.x)/2.0f) ;
	top_y = (float)(center.y - fabs(size.y)/2.0f);
	bottom_y = (float)(center.y + fabs(size.y)/2.0f);
}

bool Rect::IsWithin(const Vector &p){
	return (bool) ((p.x >= left_x) && (p.x <= right_x) && (p.y >= top_y) && (p.y <= bottom_y)) ;
}

//order: TL, TR, BR, BL

Vector Rect::GetPoint(int n){
	switch (n % 5) {
	case 0: return TopLeftCorner();
	case 1: return TopRightCorner();
	case 2: return BottomRightCorner();
	case 3: return BottomLeftCorner();
	case 4: return Center();
	}    
	my_error("Rect::GetPoint: how did I get here");
	return Vector(0,0);
}

Vector Rect::Center(){
	return Vector((left_x+right_x)/2.0f, (bottom_y+top_y)/2.0f);
}

Line Rect::LeftEdge() {
	Line l;
	l.LineFromTwoPoints(TopLeftCorner(), BottomLeftCorner());
	return l;
}

Line Rect::RightEdge() {
	Line l;
	l.LineFromTwoPoints(TopRightCorner(), BottomRightCorner());
	return l;
}

Line Rect::TopEdge() {
	Line l;
	l.LineFromTwoPoints(TopLeftCorner(), TopRightCorner());
	return l;
}

Line Rect::BottomEdge() {
	Line l;
	l.LineFromTwoPoints(BottomLeftCorner(), BottomRightCorner());
	return l;
}

Line Rect::nearestHEdgeLine(const Vector& p){
  return (fabs((p.y - top_y)) < fabs((bottom_y - p.y))) ? TopEdge() : BottomEdge() ;
}

Line Rect::nearestVEdgeLine(const Vector& p){
  return (fabs((p.x - left_x)) < fabs((right_x - p.x))) ? LeftEdge() : RightEdge() ;
}

Line Rect::nearestEdgeLine(const Vector& p) {
  if(Min((float)fabs((p.x-left_x)),(float)fabs((right_x-p.x))) < Min((float)fabs((p.y-top_y)),(float)fabs((bottom_y-p.y))))
    return nearestVEdgeLine(p) ;
  else
    return nearestHEdgeLine(p) ;  
}

bool Rect::RayIntersection(Ray r,Vector &p){
	Vector crosspoint;
	Vector returnp;
	float crossdist = -1;
	if (r.intersection(LeftEdge(),crosspoint))
	{
		if (crosspoint.y >=top_y && crosspoint.y <= bottom_y && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(RightEdge(),crosspoint))
	{
		if (crosspoint.y >=top_y && crosspoint.y <=bottom_y && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(TopEdge(),crosspoint))
	{
		if (crosspoint.x >= left_x && crosspoint.x <= right_x && (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (r.intersection(BottomEdge(),crosspoint))
	{
		if (crosspoint.x >=left_x && crosspoint.x <= right_x&& (r.DistanceFromOrigin(crosspoint) < crossdist || crossdist == -1))
		{
			crossdist = r.DistanceFromOrigin(crosspoint);
			returnp = crosspoint;
		}
	}
	if (crossdist  != -1)
	{
		p = returnp;
		return true;
	}
	else
	{
		p = Vector(0,0);
		return false;
	}
}