#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 = 0Vector2d 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 VectorVector3d 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 ) ) );}