geometry.cpp

2002年

#include "geometry.h"
#include "smp_func.h"

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

float Ray::Allowed_AngleDiff(float distance) const{
	return 20.0f;	//simplified by yjy ( for it's enough to use)
	//return float(10 - 5*exp(-distance * 0.2f));
}

float Ray::Allowed_AngleDiff(const Vector& point) const{
	return Allowed_AngleDiff(point.dist(origin));
}

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

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

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

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

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

bool Ray::intersection(const Ray& r, Vector &pPt) const
{
	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(const Line& l) const{
	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, const Vector& center, Vector& psol1, Vector& psol2) const
{
	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{
			return 0;
		}
	}
	else if (t2 > 0.0) {
		psol1 = origin + direction * t2;
		psol2 = psol1;
		return 1;
	}
	else
		return 0;
	return 0;
}

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

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

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

float Ray::DistanceFromOrigin(const Vector& point) const{
	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(const Vector& pt1, const Vector& pt2)
{
	float temp = (pt2.x - pt1.x);
	if (fabs(temp) < FLOAT_EPS) {
		//if (fabs(pt2.y - pt1.y) < FLOAT_EPS) ;
		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(const Ray& r)   
{
	//if (fabs(r.direction.y) < FLOAT_EPS && fabs(r.direction.x) < FLOAT_EPS)

	LineFromTwoPoints(r.origin, r.origin + r.direction);
}

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

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

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

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

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

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

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

float Line::get_y(float x) const{
	if ( B != 0 ) 
		return (-A*x - C)/B;

	return 0;
}

float Line::get_x(float y) const{
	if ( A != 0 ) 
		return (-B*y - C)/A;

	return 0;
}

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

bool Line::InBetween(const Vector& point, const Vector& end1, const Vector& end2) const{
//	if (!OnLine(end1) || !OnLine(end2))

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

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

Vector Line::GetClosestPtInBetween(const Vector& pt, const Vector& end1, const Vector& end2) const{
	//if (!OnLine(end1) || !OnLine(end2))

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


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

Vector Line::intersection(const Line& l) const{
	Vector result = 0;
	if (SameSlope(l)) {
		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(const Ray& r, Vector &ppt) const{
	Line lRay(r);
	
	if (SameSlope(lRay))
		return false;
	
	ppt = intersection(lRay);
	return (r.InRightDir(ppt));
}

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

	return p1.dist(target_point) < p2.dist(target_point);
}


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

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

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

int Line::CircleIntersect(float radius, const Vector& center, Vector &psol1, Vector& psol2) const
{
	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) const
{
	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) const
{
	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() const
{
	return Vector((left_x+right_x)/2.0f, (bottom_y+top_y)/2.0f);
}

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

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

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