geometry.h

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

#ifndef MSPACEGEOMETRY_H
#define MSPACEGEOMETRY_H
#include <iostream>
#include <cmath>
#define PAI 3.1415926
//add your include here
namespace MSpace {

/**
this class is used for geometry computation.
*/
class  Pol//极坐标
{
public:
     Pol(){Px=Py=Pz=0;}
     Pol(double x,double y,double z){Px=x,Py=y,Pz=z; }
     void operator=(const Pol& p){Px=p.Px,Py=p.Py,Pz=p.Pz;}
     bool operator==(const Pol& p){return (Px==p.Px)&&(Py==p.Py)&&(Pz==p.Pz);}
     Pol  operator+(const Pol& p){return Pol(Px+p.Px,Py+p.Py,Pz+p.Pz);}
     Pol  operator-(const Pol& p){return Pol(Px-p.Px,Py-p.Py,Pz-p.Pz);}
     friend  std::ostream& operator<<(std::ostream& out,Pol p)
     {
         out<<'('<<p.Px<<' '<<p.Py<<' '<<p.Pz<<')';
         return out;
     }
     double Px;
     double Py;
     double Pz;
     //~Pol();
};
class  Vec//绝对坐标
{
public:
     	Vec(){Vx=Vy=Vz=0;}
     	Vec(double x,double y,double z){Vx=x,Vy=y,Vz=z; }
     	void operator=(const Vec& v){Vx=v.Vx,Vy=v.Vy,Vz=v.Vz;}
     	bool operator==(const Vec& v){return (Vx==v.Vx)&&(Vy==v.Vy)&&(Vz==v.Vz);}
     	Vec  operator+(const Vec& v){return Vec(Vx+v.Vx,Vy+v.Vy,Vz+v.Vz);}
	Vec  operator*(const double f){return Vec(Vx*f,Vy*f,Vz*f);}
	Vec  operator/(const double f){return Vec(Vx/f,Vy/f,Vz/f);}
     	Vec  operator-(const Vec& v){return Vec(Vx-v.Vx,Vy-v.Vy,Vz-v.Vz);}
	double Multiply(const Vec& vector1,const Vec& vector2){return (vector1.Vx*vector2.Vx+vector1.Vy*vector2.Vy+vector1.Vz*vector2.Vz);}
	bool VectorisParallel(Vec vector)
		{return(((Vy*vector.Vz-Vz*vector.Vy)==0)&&((Vz*vector.Vx-Vx*vector.Vz)==0)&&((Vx*vector.Vy-Vy*vector.Vx)==0));}
	double GetMod(){return sqrt(sqr(Vx)+sqr(Vy)+sqr(Vz));}
	double GetXYAngel();
	double GetZAngel();
	void  SetMod(double mod)
	{
		Vx=Vx/GetMod()*mod;
		Vy=Vy/GetMod()*mod;
		Vz=Vz/GetMod()*mod;
	}
	Vec Point2Vector(const Vec v1,const Vec v2);
	Vec Angel2Vector(double dDist,double angel1,double angel2);
     	friend  std::ostream& operator<<(std::ostream& out,Vec &v)
     	{
         	out<<'('<<v.Vx<<' '<<v.Vy<<' '<<v.Vz<<')';
         	return out;
     	}
     	double Vx;
     	double Vy;
     	double Vz;
private:	
	double sqr(double x){return x*x;}
    	// ~Vec();
};
class Angel//角度
{
public:
   	Angel(){angel=0;}
   	Angel(double Fang) {
		while(Fang>180)
			Fang=Fang-360;
		while(Fang<-180)	
       			Fang=360+Fang;
		angel=Fang;}
   	void get_in180(Angel& Aang);
   	Angel operator+(const Angel Aang);
   	Angel operator-(const Angel Aang);
   	Angel operator*(const double Fang);
   	Angel operator/(const double Fang);
   	void  operator=(Angel Aang){angel=Aang.angel;}
   	bool  operator==(const Angel Aang){return(angel==Aang.angel);}
   //重载所有的＋，－，＊，/，＝,==,>,<等运算注意角度的取值范围（－180，180）例如：2＊100＝2＊100－360＝-160；支持 double 和 Angel的运算。
   	double angel;
};

class Line//这里是射线，(X,Y)表示射线与球场的交点。
{
public:
  	Line(){}
  	Line(Vec V1,Vec V2){vec=V1;vector=V2;}
	Line operator=(Line line){vec=line.vec;vector=line.vector;}
	bool operator==(Line line){return(vec==line.vec)&&(vector==line.vector);}
	bool LineisParallel(Line line){return vector.VectorisParallel(line.vector);}
	friend  std::ostream& operator<<(std::ostream& out,Line &line)
     	{
         	out<<'('<<line.vec.Vx<<' '<<line.vec.Vy<<' '<<line.vec.Vz<<')'<<','<<'('<<line.vector.Vx<<' '<<line.vector.Vy<<' '<<line.vector.Vz<<')';
         	return out;
     	}
	Vec vector;//向量
	Vec vec;//点
};

class Geometry{
public:
    Geometry();
    Vec Pol_Vec(const Pol p);//将极坐标转为绝对坐标
    Pol Vec_Pol(const Vec v);//将绝对坐标转为极坐标
   
    double GetDistance(const Vec v1,const Vec v2);
    //返回点v1到v2的距离
    Line  MakeLine(const Vec v1,const Vec v2);
    //作一条从v1到v2的射线，注意特殊角度的处理。
    Vec   GetComVec(const Line line1,const Line line2);
    //如果他们不能在空间相交，则返回它们在球场平面上的投影线的交点。如果投影线还不能相交则返回（0，0，0）；
    Vec  GetFoot(const Vec v,const Line line);
    //返回点v到线line的垂足点。
    ~Geometry();
private:
	double sqr(double x){return x*x;}

};

}

#endif