function.cpp

一个简单的基础机器人足球程序

#include "StdAfx.h"
#include "function.h"
#include <math.h>

//计算两点之间距离
double Distance (Vector3D pos1,Vector3D pos2)
{
	return sqrt( (pos1.x-pos2.x)*(pos1.x-pos2.x) + (pos1.y-pos2.y)*(pos1.y-pos2.y) );
}

double Cut(double num,double down,double up)
{
	if(num<down)
		return down;
	if(num>up)
		return up;
	return num;
}


long Cut(long num,long down,long up)
{
	if(num<down)
		return down;
	if(num>up)
		return up;
	return num;

}



//规范角度
void NormalAngle(double &angle)
{
	while(angle >= 180.0 )angle-=360.0;
	while(angle < -180.0 )angle+=360.0;
}

//转换 ？？？？四舍五入
int toint(double num)
{
 	return num*10 < (int)num*10+5?(int)num:(int)num+1;
}

// 求角度
double Atan(double y,double x)
{
	if(fabs(x)<0.01){
		if(y>0)
			return 90;
		else
			return -90;
	}

	if(fabs(y)<0.01){
		if(x>0)
			return 0;
		else
			return 180;
	}
	return 180*atan2(y,x)/PI;
}

//求两点之间的角度
double Atan(Vector3D begin,Vector3D end)
{
	double y,x;
	y=end.y - begin.y ;
	x=end.x - begin.x ;
	return Atan(y,x);
}

//返回角度的变化后的值
double AngleOne(double e,double vl,double vr)
{
	vl=Cut(vl,-125,125);
	vr=Cut(vr,-125,125);

	double angle = (vr - vl)/2;
	NormalAngle(e);		

	e += 0.273575*(0.534262* angle-e);

	if( vr > vl )
	{	
		if( vl >= 0 || vr <=0 )
		{
			e -= 4 * 0.000294678 * angle * angle;
		}
	}
	else if( vr < vl )
	{	
		if( vr >= 0 || vl <=0 )
		{
			e += 4 * 0.000294678 * angle * angle;
		}
	}

	NormalAngle(e);	

	return e;

}

//返回速度的变化后的值
double VelocityOne(double speed,double vl,double vr)
{
	vl=Cut(vl,-125,125);
	vr=Cut(vr,-125,125);

	if(speed > 3 || speed < -3)
		speed =0;

	if( vl==0 && vr==0 )
		speed += -SPEED_ODD * speed;
	else
		speed += 0.060*( 0.015222305*(vl +vr)/2-speed);
	return speed;
}


////求取速度 或者位移 
Speed operator -(Vector3D &v1,Vector3D &v2)
{
	Speed s;
	s.x=v1.x-v2.x;
	s.y=v1.y-v2.y;
	s.z=Atan(s.y,s.x);
	NormalAngle(s.z);
	s.v=sqrt(s.x*s.x+s.y*s.y);
	return s;
}

//返回两点之间的连线和一直线（x=？）的焦点的y值
double meet_y(Vector3D p1,Vector3D p2,double x)
{

	if(fabs(p2.x-p1.x)<0.1)
		return x;
	return p1.y+(p2.y-p1.y)*(x-p1.x)/(p2.x-p1.x);
}


//已知一个点和斜率  返回这个直线 和一垂直直线（x=？）的焦点的y值
double meet_y(Vector3D p,double z,double x)
{

	if(fabs(fabs(z)-90)<0.1)
		return x;
	return p.y+tan(z*PI/180)*(x-p.x);
}