geometry.cpp~

底层robocup3d源码。 suse１０．０ kdevelop 开发.

#include "geometry.h"
MSpace::Vec *Vp;
namespace MSpace {
void Angel::get_in180(Angel& Aang)
{
   	while(Aang.angel>180)
		Aang.angel=Aang.angel-360;
	while(Aang.angel<-180)	
       		Aang.angel=360+Aang.angel;	
}
Angel Angel::operator+(const Angel Aang)
{
      	Angel result;
	result.angel=angel+Aang.angel;
	get_in180(result);
	return result;
}
Angel Angel::operator-(const Angel Aang)
{
	Angel result;
	result.angel=angel-Aang.angel;
	get_in180(result);
	return result;
}
Angel Angel::operator*(const double Fang)
{
	Angel result;
	result.angel=angel*Fang;
	get_in180(result);
	return result;
}
Angel Angel::operator/(const double Fang)
{
	Angel result;
	result.angel=angel/Fang;
	get_in180(result);
	return result;
}
double Vec::GetXYAngel()
{
	double test;
 	if(Vx==0)
 	{
		if(Vy==0)
 			return 0;
 		else if(Vy<0) return -90;
 		else	return 90;
 	}
	else if(Vx>0)return (atan(Vy/Vx))*180/PAI;
 	else
 	{
		if(Vy>0){return (180+(atan(Vy/Vx))*180/PAI);}
		else if(Vy<0)return (-180+(atan(Vy/Vx))*180/PAI);
		else return -180;
 	}
}
double Vec::GetZAngel()
{
	double M;
	M=GetMod();
	return (asin(Vz/M))*180/PAI;
}
Vec Vec::Point2Vector(const Vec v1,const Vec v2)
{
         Vec vector,point;
	point=v2;
	vector=point-v1;
	return vector;
}
Vec Vec::Angel2Vector(double dDist,double XYAngel1,double ZAngel2)
{
	Vec vector;
	double R;
	R=dDist*cos(ZAngel2*PAI/180);
	vector.Vx=R*cos(XYAngel1*PAI/180);
	vector.Vy=R*sin(XYAngel1*PAI/180);
	vector.Vz=dDist*sin(ZAngel2*PAI/180);
	return vector;	
}
Geometry::Geometry()
{
}

Vec Geometry::Pol_Vec(const Pol p)
{
	Vec v;
	double R;
	R=p.Px*cos(p.Pz*PAI/180);
	v.Vx=R*cos(p.Py*PAI/180);
	v.Vy=R*sin(p.Py*PAI/180);
	v.Vz=p.Px*sin(p.Pz*PAI/180);
	return v;
}
Pol Geometry::Vec_Pol(const Vec v)
{
	Pol p;
	Vec Zero;
	p.Px=GetDistance(v,Zero);
	if(v.Vx==0)
	{
		if(v.Vy==0)
			p.Py=0;
		else if(v.Vy<0)
			p.Py=-90;
		else	p.Py=90;
	}
	else if(v.Vx>0)
		p.Py=(atan(v.Vy/v.Vx))*180/PAI;
	else
	{
		if(v.Vy>0)
			p.Py=90-(atan(v.Vy/v.Vx))*180/PAI;
		else	p.Py=-90-(atan(v.Vy/v.Vx))*180/PAI;
	}
	if(p.Px==0)
		p.Pz=0;
	else	p.Pz=asin(v.Vz/p.Px)*180/PAI;
	return p;
	
}
double Geometry::GetDistance(const Vec v1,const Vec v2)
{
	return(sqrt(sqr(v1.Vx-v2.Vx)+sqr(v1.Vy-v2.Vy)+sqr(v1.Vz-v2.Vz)));
}
Line Geometry::MakeLine(const Vec v1,const Vec v2)
{
	Line line;
	Vec v=v2;
	line.vec=v1;
	line.vector=v-v1;
	return line;
}
Vec Geometry::GetComVec(const Line line1,const Line line2)
{
	Vec Point;
	Point.Vx=(line2.vector.Vx*line1.vec.Vx-line1.vector.Vx*line2.vec.Vx)/(line2.vector.Vx-line1.vector.Vx);
	Point.Vy=(line2.vector.Vy*line1.vec.Vy-line1.vector.Vy*line2.vec.Vy)/(line2.vector.Vy-line1.vector.Vy);
	Point.Vz=0;
	return Point;	
}
Vec Geometry::GetFoot(const Vec v,const Line line)
{
	double R,Rel_M,S_M;
	Vec Point,Vector1,Vector2;
	Vector1=Vp->Point2Vector(line.vec,v);
	Vector2=line.vector;
	R=Vp->Multiply(Vector1,Vector2);
	S_M=Vector2.GetMod();
	Rel_M=R/S_M;
	Point.Vx=line.vec.Vx+Rel_M*line.vector.Vx/S_M;
	Point.Vy=line.vec.Vy+Rel_M*line.vector.Vy/S_M;
	Point.Vz=line.vec.Vz+Rel_M*line.vector.Vz/S_M;
	return Point;
}
Geometry::~Geometry()
{
}


}