geometry.cpp

robocuo相关资料robocuo相关资料

#include "Geometry.h"
#include "Matrix.h"

CS PrintSystem;

bool CMP::eq(double a, double b)
{
	return fabs(a-b)<EPS;
}

bool CMP::ne(double a, double b)
{
	return fabs(a-b)>=EPS;
}

bool CMP::lt(double a, double b)
{
	return b-a>=EPS;
}

bool CMP::gt(double a, double b)
{
	return a-b>=EPS;
}

bool CMP::le(double a, double b)
{
	return b-a>-EPS;
}

bool CMP::ge(double a, double b)
{
	return a-b>-EPS;
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-----------------------------------> Vector2d <---------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Vector2d::Vector2d ()
:x(0), y(0){}

Vector2d::Vector2d(const Vector3d &v)
:x(v.X()), y(v.Y())
{
}

Vector2d::Vector2d (double a, double b, CS _cs)
{
	if(_cs==CARTESIAN)
	{
		x=a;
		y=b;
	}
	else
	{
		x=a* Angle::cosDeg(b);
		y=a* Angle::sinDeg(b);
	}
}

void Vector2d::setVector( double a, double b, CS _cs )
{
	if(_cs==CARTESIAN)
	{
		x=a;
		y=b;
	}
	else
	{
		x=a* Angle::cosDeg(b);
		y=a* Angle::sinDeg(b);
	}
}

double & Vector2d::X()
{
	return x;
}

double & Vector2d::Y()
{
	return y;
}

Vector2d Vector2d::operator-()
{
	return Vector2d(-x,-y);
}

Vector2d Vector2d::operator+(Vector2d p)
{
	return Vector2d(x+p.x, y+p.y);
}

Vector2d Vector2d::operator+=(Vector2d p)
{
	return Vector2d(x+=p.x, y+=p.y);
}

Vector2d Vector2d::operator-(Vector2d p)
{
	return Vector2d(x-p.x, y-p.y);
}

Vector2d Vector2d::operator-=(Vector2d p)
{
	return Vector2d(x-=p.x, y-=p.y);
}

Vector2d Vector2d::operator*(double d)
{
	return Vector2d(x*d, y*d);
}

Vector2d Vector2d::operator*=(double d)
{
	return Vector2d(x*=d, y*=d);
}

Vector2d Vector2d::operator*( Vector2d p )
{
	return Vector2d( x * p.x, y * p.y );
}

Vector2d Vector2d::operator*=( Vector2d p )
{
	return Vector2d( x *= p.x, y *= p.y );
}

Vector2d Vector2d::operator/( Vector2d p )
{
	if( CMP::eq( p.y, 0 ) )
		return p;

	return Vector2d( x / p.x, y / p.y );
}

Vector2d Vector2d::operator/=( Vector2d p )
{
	if( CMP::eq( p.y, 0 ) )
		return p;

	return Vector2d( x /= p.x, y /= p.y );
}

// Divid Vector by a constant
// param d double number to divide by
// throws string throws a string when d = 0

Vector2d Vector2d::operator/(double d)
{
	if(CMP::eq(d,0))
		cerr << string("Vector2d::operator/") << endl;
	return Vector2d(x/d, y/d);
}

Vector2d Vector2d::operator/=(double d)
{
	if(CMP::eq(d,0))
		cerr << string("Vector2d::operator/=") << endl;
	return Vector2d(x/=d, y/=d);
}

bool Vector2d::operator==(Vector2d p)
{
	return CMP::eq(x,p.x) && CMP::eq(y,p.y);
}

bool Vector2d::operator!=(Vector2d p)
{
	return CMP::ne(x,p.x) || CMP::ne(y,p.y);
}

double Vector2d::magnitude() const
{
	return sqrt(x*x+y*y);
}

void Vector2d::setMagnitude(double d)
{
	if(CMP::eq(x*x+y*y,0))
		cerr << string("Vector2d::setMagnitude") << endl;
	*this *= d/magnitude();
}

double Vector2d::angle() const
{
	if(CMP::eq(x,0) && CMP::eq(y,0))
		cerr << string("Vector2d::angle") << endl;

	return Angle::normalizeAngle(Angle::atan2Deg(y,x));
}

double Vector2d::angleTo(Vector2d v)
{
	return Angle::normalizeAngle(angle()-v.angle());
}

double Vector2d::distance(Vector2d p)
{
	return (*this-p).magnitude();
}
void Vector2d::rotate(double a)
{
	double tx = x*Angle::cosDeg(a) - y*Angle::sinDeg(a);
	double ty = y*Angle::sinDeg(a) + y*Angle::cosDeg(a);
	x = tx;
	y = ty;
}

Vector2d Vector2d::rotate(Vector2d c, double d)
{
	Vector2d a,b=*this-c;
	a.x= b.x* Angle::cosDeg(d)-b.y* Angle::sinDeg(d);
	a.y= b.x* Angle::sinDeg(d)+b.y* Angle::cosDeg(d);
	return *this=a+c;
}

Vector2d Vector2d::rotate(Vector2d c, double Sin, double Cos)
{
	Vector2d a,b=*this-c;
	a.x= b.x*Cos - b.y*Sin;
	a.y= b.x*Sin + b.y*Cos;
	return *this=a+c;
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-----------------------------------> Vector3d <---------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Vector3d operator * ( double d, const Vector3d &v)
{
	return ( Vector3d( v.X() * d, v.Y() * d, v.Z() * d  ) );
}

Vector3d::Vector3d (double a , double b , double c , CS cs)
{
	if(cs==CARTESIAN)
	{
		mX=a;
		mY=b;
		mZ=c;
	}
	else
	{
		mX = a * Angle::cosDeg( c ) * Angle::cosDeg ( b ) ;
		mY = a * Angle::cosDeg( c ) * Angle::sinDeg ( b ) ;
		mZ = a * Angle::sinDeg( c )  ;
	}
}

void Vector3d::setVector( double a , double b , double c , CS cs )
{
	if(cs==CARTESIAN)
	{
		mX=a;
		mY=b;
		mZ=c;
	}
	else
	{
		mX = a * Angle::cosDeg( c ) * Angle::cosDeg ( b ) ;
		mY = a * Angle::cosDeg( c ) * Angle::sinDeg ( b ) ;
		mZ = a * Angle::sinDeg( c )  ;
	}
}

Vector2d Vector3d :: get_vector2d()
{
	return Vector2d(mX,mY);
}
Vector3d::Vector3d (Vector2d p)
{
	mX=p.x;
	mY=p.y;
	mZ=0;
}

void Vector3d::setVector( Vector2d p )
{
	mX=p.x;
	mY=p.y;
	mZ=0;
}

Vector3d Vector3d::operator-()
{
	return Vector3d(-mX, -mY, -mZ);
}

Vector3d Vector3d::operator+(Vector3d p) const
{
	return Vector3d(mX+p.mX, mY+p.mY, mZ+p.mZ);
}

Vector3d Vector3d::operator+=(Vector3d p)
{
	return Vector3d(mX+=p.mX, mY+=p.mY, mZ+=p.mZ);
}

Vector3d Vector3d::operator-(Vector3d p) const
{
	return Vector3d(mX-p.mX, mY-p.mY, mZ-p.mZ);
}

Vector3d Vector3d::operator-=(Vector3d p)
{
	return Vector3d(mX-=p.mX, mY-=p.mY, mZ-=p.mZ);
}

Vector3d Vector3d::operator*(double d) const
{
	return Vector3d(mX*d, mY*d, mZ*d);
}

Vector3d Vector3d::operator*(Vector3d p) const
{
	return Vector3d(mX*p.mX, mY*p.mY, mZ*p.mZ);
}

// operator % : Cross multiplication of two Vector
Vector3d Vector3d::operator % ( const Vector3d &p          )const
{
	return Vector3d (mY * p.mZ - p.mY * mZ, p.mX * mZ - mX * p.mZ, mX * p.mY - p.mX * mY);
}


Vector3d Vector3d::operator*=(double d)
{
	return Vector3d(mX*=d, mY*=d, mZ*=d);
}

double Vector3d::operator &(const Vector3d &p) const
{
	return ( mX * p.mX + mY * p.mY + mZ * p.mZ );
}

Vector3d Vector3d::operator/(double d) const
{
	if(CMP::eq(d,0))
		cerr<<string("Vector3d::operator/")<<endl;
	return Vector3d(mX/d, mY/d, mZ/d);
}

Vector3d Vector3d::operator / (Vector3d p )const
{
	return ( Vector3d( mX / p.mX, mY / p.mY, mZ / p.mZ ) );
}

Vector3d Vector3d::operator=(double d)
{
	mX = mY = mZ = d;
}

Vector3d Vector3d::operator /= ( double d )
{
	if(CMP::eq(d,0))
		cerr<<string("Vector3d::operator/=")<<endl;
	return Vector3d(mX/=d, mY/=d, mZ/=d);
}

void Vector3d::operator /=(Vector3d &p )
{
	if ( p.mX != 0 )
		mX /= p.mX;
	else
		cerr << "(Vector3d::operator /=) divide by zero in X factor" << endl;
	if ( p.mY != 0 )
		mY /= p.mY;
	else
		cerr << "(Vector3d::operator /=) divide by zero in Y factor" << endl;
	if ( p.mZ != 0 )
		mZ /= p.mZ;
	else
		cerr << "(Vector3d::operator /=) divide by zero in Z factor" << endl;
}

bool Vector3d::operator==(Vector3d p)
{
	return CMP::eq(mX,p.mX) && CMP::eq(mY,p.mY) && CMP::eq(mZ,p.mZ);
}

bool Vector3d::operator!=(Vector3d p)
{
	return CMP::ne(mX,p.mX) || CMP::ne(mY,p.mY) || CMP::ne(mZ,p.mZ);
}

double & Vector3d::X()
{
	return mX;
}

double Vector3d::X() const
{
	return mX;
}

double & Vector3d::Y()
{
	return mY;
}

double  Vector3d::Y() const
{
	return mY;
}

double & Vector3d::Z()
{
	return mZ;
}

double Vector3d::Z() const
{
	return mZ;
}

double Vector3d::theta()const
{
	return Angle::normalizeAngle2(Angle::atan2Deg(mY,mX));
}

double Vector3d::phi()const
{
	if(CMP::eq(magnitude(),0))
		cerr<<string("Vector3d::phi")<<endl;
	return Angle::normalizeAngle2(Angle::asinDeg(mZ/magnitude()));
}

double Vector3d::magnitude() const
{
	return sqrt(sqr(mX)+sqr(mY)+sqr(mZ));
}

Vector3d Vector3d::getNormalized() const
{
	Vector3d vec(*this);
	vec.setMagnitude(1);
	return vec;
}

Vector3d Vector3d::normalize()
{
	setMagnitude(1);
	return *this;
}

double Vector3d::getDistanceTo(Vector3d vec)
{
	return (*this - vec).magnitude();
}

void Vector3d::setTheta(double theta)
{
	*this=Vector3d(magnitude(),theta, phi(),POLAR);
}

void Vector3d::setPhi(double phi)
{
	*this=Vector3d(magnitude(),theta(), phi,POLAR);
}

void Vector3d::setMagnitude(double magnitude)
{
	*this=Vector3d(magnitude,theta(), phi(),POLAR);
}

AngDeg Vector3d::Angle(Vector3d vector)
{
	if(CMP::eq(magnitude(),0)||CMP::eq(vector.magnitude(),0))
		cerr<<string("Vector::Angle")<<endl;
	double im=mX*vector.mX + mY*vector.mY + mZ*vector.mZ;
	return Angle::acosDeg(im/(magnitude()*vector.magnitude()));
}

Vector3d Vector3d::rotate2d( double ang )
{
	double dMag    = magnitude();
	double dNewDir = theta() + ang;

	mX = dMag * Angle::cosDeg(dNewDir);
	mY = dMag * Angle::sinDeg(dNewDir);

	return *this;
}

Vector3d Vector3d::rotate( double ang )
{
	double dMag    = magnitude();
	double dNewDir = theta() + ang;
	setVector( dMag, dNewDir, POLAR );
	return ( *this );
}

double Vector3d::ang()
{
	double mod = mX * mX + mY * mY;
	if( mod > 0 )
	{
		double r = sqrt( mod );
		return ( mY >= 0 ?  acos( mX / r ) : - acos( mX / r ) ) * 180 / Pi;
	}
	return 0;
}

Vector3d::operator Vector2d ()
{
	return Vector2d(mX,mY);
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-------------------------------------> Angle <----------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

AngDeg Angle::normalizeAngle( AngDeg angle )
{
	while( angle >= 360.0  ) angle -= 360.0;
	while( angle < 0.0 ) angle += 360.0;

	return ( angle );
}

AngDeg Angle::normalizeAngle2( AngDeg angle )
{
	while( angle > 180.0  ) angle -= 360.0;
	while( angle < -180.0 ) angle += 360.0;

	return ( angle );
}

double Angle::minAngle(AngDeg a, AngDeg b)
{
	if(Max(a,b)-Min(a,b)<=180)
		return Min(a,b);
	return Max(a,b);
}

double Angle::maxAngle(AngDeg a, AngDeg b)
{
	if(Max(a,b)-Min(a,b)<=180)
		return Max(a,b);
	return Min(a,b);
}

int Angle::cmpAngle(AngDeg a, AngDeg b)
{
	a=normalizeAngle(a);
	b=normalizeAngle(b);
	if(CMP::eq(a,b))
		return 0;
	if(Max(a,b)-Min(a,b)<=180)
	{
		if(CMP::lt(a,b))
			return -1;
		if(CMP::gt(a,b))
			return 1;
	}
	if(CMP::lt(a,b))
		return 1;
	if(CMP::gt(a,b))
		return -11;
}

AngDeg Angle::bisectorAngle( AngDeg a, AngDeg b )
{
	a=normalizeAngle(a);
	b=normalizeAngle(b);
	if(Max(a,b)-Min(a,b)<=180)
		return (a+b)/2;
	return normalizeAngle((a+b)/2+180);
}

/*Ray Angle::bisector ( Vector2d center, Vector2d leftLeg, Vector2d rightLeg )
{
	return Ray(center, bisectorAngle( (leftLeg-center).angle(), (rightLeg-center).angle() ) );
}
*/

double Angle::pointsAngle ( Vector2d center, Vector2d leftLeg, Vector2d rightLeg )
{
	return fabs ( normalizeAngle2 (   ( leftLeg - center ).angle ()  -  ( rightLeg - center ).angle () ) );
}

bool Angle::isAngInRange( AngDeg ang1, AngDeg ang2, AngDeg ang )
{
// normalize all angles
	ang=normalizeAngle(ang);
	ang1=normalizeAngle(ang1);
	ang2=normalizeAngle(ang2);

	AngDeg angMin=minAngle(ang1, ang2);
	AngDeg angMax=maxAngle(ang1, ang2);

	if( angMin < angMax )						  // 0 ---false-- angMin ---true-----angMax---false--360
		return angMin < ang && ang < angMax ;
	else										  // 0 ---true--- angMax ---false----angMin---true---360
		return !( angMax < ang && ang < angMin );
}

bool Angle::isPointInRange( Vector2d center, Vector2d leftLeg, Vector2d rightLeg , Vector2d p)
{
	return isAngInRange((leftLeg-center).angle(),(rightLeg-center).angle(),(p-center).angle());
}

AngDeg Angle::Rad2Deg( AngRad x )
{
	return ( x * 180 / Pi );
}

AngRad Angle::Deg2Rad( AngDeg x )
{
	return ( x * Pi / 180 );
}

double Angle::cosDeg( AngDeg x )
{
	return ( cos( Deg2Rad( x ) ) );
}

double Angle::sinDeg( AngDeg x )
{
	return ( sin( Deg2Rad( x ) ) );
}

double Angle::tanDeg( AngDeg x )
{
	return ( tan( Deg2Rad( x ) ) );
}

AngDeg Angle::atanDeg( double x )
{
	return ( Rad2Deg( atan( x ) ) );
}

double Angle::atan2Deg( double x, double y )
{
	return ( Rad2Deg( atan2( x, y ) ) );
}

AngDeg Angle::acosDeg( double x )
{
	if( x >= 1 )
		cerr<<string("Angle::acosDeg")<<endl;
	else if( x <= -1 )
		cerr<<string("Angle::acosDeg")<<endl;

	return ( Rad2Deg( acos( x ) ) );
}

AngDeg Angle::asinDeg( double x )
{
	if( x >= 1 )
		cerr<<string("Angle::asinDeg")<<endl;
	else if ( x <= -1 )
		cerr<<string("Angle::asinDeg")<<endl;

	return ( Angle::Rad2Deg( asin( x ) ) );
}