geometry.h

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 <math.h>
#ifndef _Geometry
#define _Geometry	

#include "types.h"

#define FLOAT_EPS 0.001f

/********************************************************************************/
/*                        Vector Class                                          */
/********************************************************************************/
class Vector {
public:
	float x;
	float y;
  
	Vector(float x = 0, float y = 0) { this->x = x; this->y = y; }
	Vector operator -() { return Vector(-x, -y);}
	Vector operator +(const Vector &a) { return Vector(x + a.x, y + a.y); }
	Vector operator -(const Vector &a) { return Vector(x - a.x, y - a.y); }
	Vector operator *(const float &a) { return Vector(x * a, y * a); }
	Vector operator *(const Vector &a) { return Vector(x * a.x, y * a.y); }
	Vector operator /(const float &a) { return Vector(x / a, y / a); }
	Vector operator /(const Vector &a) { return Vector(x / a.x, y / a.y); }
	void operator =(const float &a) { x = a; y = a; }
	void operator +=(const Vector &a) { x += a.x;  y += a.y; }
	void operator +=(const float &a) { x += a;  y += a; }
	void operator -=(const Vector &a) { x -= a.x;  y -= a.y; }
	void operator -=(const float &a) { x -= a;  y -= a; }
	void operator *=(const float &a) { x *= a;  y *= a; }
	void operator /=(const float &a) { x /= a;  y /= a; }
	bool operator !=(const Vector &a) { return (x != a.x) || (y != a.y); } 
	bool operator !=(const float &a) { return (x != a) || (y != a); }
	bool operator ==(const Vector &a) { return (x == a.x) && (y == a.y); }
	inline float mod(){ return float(sqrt(x*x+y*y)); }
	inline float mod2() { return x*x + y*y; }//square mod
	void Normalize(){ if (mod2() != 0) (*this) /= mod(); }
	inline float disaeolus(Vector& pos){return Max((float)fabs(x-pos.x),(float)fabs(y-pos.y)); }
	inline float dis2aeolus(Vector& pos){return (float)(fabs(x-pos.x)+fabs(y-pos.y)); }
	float dist(Vector& pos) { return (*this-pos).mod(); }
	float dist2(const Vector &a) { return (*this-a).mod2(); }//square dist
	Vector Rotate(AngleDeg ang){ return Vector(Cos(ang)*x - Sin(ang)*y,Sin(ang)*x + Cos(ang)*y); }

	AngleDeg Angle(){ return ATan2(x,y); }
	bool InFrontOf(const Vector &a) { return bool(x > a.x + 1); }
	bool InFrontOf(const float &a) { return bool(x > a + 1); }
	bool Behind(const Vector &a) { return bool(x < a.x - 1); }
	bool Behind(const float &a) { return bool(x < a - 1); }	
};

inline Vector Polar2Vector(float dist, AngleDeg ang) {
	return Vector(dist * Cos(ang), dist * Sin(ang));
}

inline Vector bisector(Vector a, Vector b){
	a.Normalize();
	b.Normalize();
	Vector c = a + b;
	c.Normalize();
	return c;
}
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                       Ray Class                                              */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
class Line;
class Ray 
{
public:
	Ray(){ origin = Vector(0,0); direction = Vector(1,0); angle = 0;}
	Ray(Vector orig, float ang){ origin = orig; direction = Polar2Vector(1.0, ang); angle = ang;}
	Ray(Vector orig, Vector& dir){ origin = orig; direction = dir; angle = direction.Angle();}
	
	float Dir(){ return angle;}
	void SetValues(Vector orig, float ang){origin = orig; direction = Polar2Vector(1.0, ang); angle = ang; }
	void SetValues(Vector orig, Vector dir){origin = orig; direction = dir; angle = dir.Angle();}

	bool OnRay(Vector& point);//point on the ray
	bool InRightDir(Vector point); // more lenient than above about distance off ray 
	bool InRightDir(AngleDeg angle);
	bool InOppositeDir(Vector& point); // more lenient than above about distance off ray 
	bool InOppositeDir(AngleDeg angle);		
	AngleDeg Allowed_AngleDiff(float distance = 5.0f);//default the distance is 5.0f
	AngleDeg Allowed_AngleDiff(Vector point);
	bool intersection(Ray r, Vector&pPt);//intersection with ray
	bool intersection(Line l, Vector&pPt);//intersection with line
	float intersection(Line l);//get the normalized param of the intersection point

	Vector GetPoint(float dist){ return origin + direction * dist;}
	/*	intersect with circle	*/
	int CircleIntersect(float radius, Vector center, Vector& psol1, Vector& psol2);

	/*	get strechangle	*/
	AngleDeg StrechAngle(Vector& point);
	float DistanceFromOrigin(Vector& point);

	Vector GetClosestPoint(Vector& point);

	float GetClosestPointParam(Vector& point); // return param t, origin + direction * t = closest_point 

	void Normalize();

	friend Line;

protected:
	Vector origin;
	Vector direction;
	float  angle;
};

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                      Line Class                                              */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
class Line {
private:	
    /* Ax + By + C = 0 */
public:
	float A,B,C;
	Line(float A = 0.0, float B = 0.0, float C = 0.0);
	Line(Ray r) { LineFromRay(r); }
	/*	initialize a line from two points	*/
	void LineFromTwoPoints(Vector pt1, Vector pt2);
	/*	initialize a line from a ray	*/
	void LineFromRay(Ray r);
	/*	initialize a line from an origin and direction	*/
	void LineFromRay(Vector orig, AngleDeg dir){LineFromRay(Ray(orig, dir));}
	void LineFromRay(Vector orig, Vector dir){LineFromRay(Ray(orig, dir)); }
	void LineFrompline(Vector pos1, Vector pos2){A = 2* (pos2.x - pos1.x); B = 2 *(pos2.y - pos1.y); C= pos1.x * pos1.x - pos2.x * pos2.x + pos1.y * pos1.y - pos2.y* pos2.y;}
  
	/*	Is the point on the line	*/
	bool PointOnLine(float x, float y);
	inline bool PointOnLine(Vector point){return PointOnLine(point.x, point.y); }

	/*	distance between the point and the line	*/
	float dist(Vector point);
	float dist2(Vector point);
	float angle(); /* returns the angle of the line in [-90, 90] */
  
	/*	Is the point between end1 and end2	*/
	bool InBetween(Vector pt, Vector end1, Vector end2);
	/*	Get the closest point on the line which is between end1 and end2	*/
	Vector GetClosestPtInBetween(Vector pt, Vector end1, Vector end2);
  
	/*	more lenient than pointonline	*/
	inline bool OnLine(Vector point, float allowance = 1.0){return bool(dist2(point) < Sqr(allowance));}
  
	/*	Get Project Point	*/
	inline Vector ProjectPoint(Vector point) { return intersection(perpendicular(point)); }

	/*	get y given x	*/
	float get_y(float x);
	/*	get x given y	*/
	float get_x(float y);
	/*	intersection with a line	*/
	Vector intersection(Line l);
	/*	get perpendicular line from a point	*/
	inline Line perpendicular(Vector point) { return Line(B,-A,A*point.y - B*point.x); }

	/*	Is the ray intersected with the line	*/
	bool RayIntersection(Ray r, Vector& ppt);
	/*	Intersection with a circle	*/
	int CircleIntersect(float radius, Vector& center, Vector& psol1, Vector& psol2);

	Line shift_y(float t){ return Line(A, B, C - t * B); }
	Line shift_x(float t){ return Line(A, B, C - t * A); }  

  /* returns whether the projection of pt1 is closer to targ_pt than the
     projection of pt2 */
	bool IsPtCloserToPtOnLine(Vector pt1, Vector pt2, Vector targ_pt);
  
	bool HalfPlaneTest(Vector pt); //return true on top/left part of plane
	bool SameSlope(Line l);

	friend Ray;
} ;


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                      Class Rect                                         */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
class Rect {
private:
	float left_x ;
	float right_x ;
	float top_y ;
	float bottom_y ;
public:
	Vector Center();

	Rect() ;
	Rect(float left,float right, float top, float bottom) ;
	Rect(const Vector& center, const Vector& size) ;
		  
	inline float TopY()     { return top_y; }
	inline float BottomY()  { return bottom_y; }
	inline float RightX()   { return right_x; }
	inline float LeftX()    { return left_x; }

	inline Vector TopLeftCorner() { return Vector(left_x, top_y); }
	inline Vector TopRightCorner() { return Vector(right_x, top_y); }
	inline Vector BottomLeftCorner() { return Vector(left_x, bottom_y); }
	inline Vector BottomRightCorner() { return Vector(right_x, bottom_y); }

	Vector GetPoint(int n); //order: TL, TR, BR, BL; starting at 0
	inline float Width()    { return right_x - left_x; }   
	inline float Height()   { return bottom_y - top_y; }  
	bool   IsWithin(const Vector& p) ;

	Line LeftEdge();
	Line RightEdge();
	Line TopEdge();
	Line BottomEdge();

	Line nearestHEdgeLine(const Vector& p);
	Line nearestVEdgeLine(const Vector& p);
	Line nearestEdgeLine(const Vector& p);
	bool RayIntersection(Ray r,Vector &p);
} ;

#endif