kick.h

robocup源代码2001年清华机器人源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#ifndef _kick
#define _kick
#include "types.h"
#include "command.h"

#define KICKDIS 5
#define KICKAGL 24

struct OApos{
	Vector pos;
	int idx_dis;
	int idx_agl;
	bool enabled;
	float qvalue[KICKDIS*KICKAGL];
	OApos* nextp[KICKDIS*KICKAGL+1];
};

struct kstate{
public:
	kstate():pos(NULL),subnode(0){};
	kstate(OApos* p):subnode(0){pos = p;}

	OApos* pos;
	int depth;
	int subnode;
	bool operator == (kstate t){return bool(pos->idx_dis == t.pos->idx_dis && pos->idx_agl == t.pos->idx_agl );}
	OApos* Subnode(){return pos->nextp[subnode];}
};

class MyOA : public OptimalA<kstate>{	
private:
	Vector s_pos,s_vel,p_vel[CP_OA_maxdepth];
	float bdfacing;
	OApos oapos[KICKDIS][KICKAGL];
	Vector Tvel;
	Vector dvel;

	int maxdepth;
	float stepcost;

	void Extend(OAnode<kstate>* pnode);
public:	
	MyOA(){maxdepth = CP_OA_maxdepth; stepcost = CP_OA_stepcost; maxscounts = CP_OA_maxsteps;}

	inline int getdisidx(float balldis){return Constrain(int((balldis-SP_ball_size-SP_player_size)/SP_kickable_margin*KICKDIS), 0, KICKDIS-1);}
	inline int getaglidx(float ballagl){return round(NormalizeAngle(ballagl,0)/360.0f*KICKAGL) % KICKAGL;}	
	inline OApos* getkickpos(int disidx,int aglidx){return &(oapos[disidx][aglidx]);}
	inline void Disable(int disidx, int aglidx){oapos[disidx][aglidx].enabled = false;}
	inline void Enable(int disidx, int aglidx){oapos[disidx][aglidx].enabled = true;}
	void Enableall();
	Vector Getfirstkick();
	Vector Getnextpos();
	inline int Getneedkicks(){return curnode == NULL ? 0 : curnode->data.depth;}
	inline int Getsearchcounts(){return scounts;}
	inline Vector GetTvel(){return Tvel;}
	void Initvpos(char* filename = NULL);	
	//float fEvaluate(kstate s);	
	void SetInitState(Vector pos, Vector vel, Vector pvel, float bodyfacing);
	inline void SetDesirevel(Vector desirevel){dvel = desirevel;}
	inline void SetMaxdepth(int depth){maxdepth = depth>CP_OA_maxdepth ? CP_OA_maxdepth:depth;}
	inline void Setstepcost(float cost){stepcost = cost;}
	inline void Setmaxsteps(int maxsteps){maxscounts = maxsteps;}

	bool Canreachpos(Vector& bpos, Vector& bvel, float bdfacing, Vector& next_bpos, float* effort=NULL);
	bool Canreachvel(Vector& bpos, Vector& bvel, float bdfacing, Vector& next_bvel, float* effort=NULL);

	void Extendcurnode();
	bool IsTargetState();
};

class Kick{
private:
	/*	wise kick */
	float speedinc[19];
	AngleDeg maxinc_angle[19];
	float getspeedinc(AngleDeg startangle, AngleDeg turnangle);
	float getmaxincangle(AngleDeg startangle);
/*	oakick	*/
	inline void Enablepos(int disidx, int aglidx);
	inline void Disablepos(int disidx, int aglidx);
	void Oa_limitpos(float rel_angle);

	Time oainf_time;
	bool oainf_allenabled;
	float oainf_Maxy, oainf_Miny, oainf_Minx, oainf_Maxx;
	bool line_limit;
	int pos_limit;
	Vector oainf_pos[3];
	float oainf_radius[3];
public:
	Kick();
/*	oa kick	*/
	MyOA KickOAlast;
	MyOA KickOA2_5;
	MyOA KickOA2_0;
	MyOA KickOA1_5;

	void SetOainf();
	void Enableall();

	KT_Res oakick(float kickspeed, AngleDeg kickangle, KK_Mode kk_mode = KK_slow);
	KT_Res oakick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode kk_mode = KK_slow);

/*	smartkick	*/
	float smartkick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode mode = KK_slow);
	float smartkick(float kickspeed, AngleDeg kickangle, KK_Mode mode = KK_slow); // execute at once

/*	turn_ball	*/
	KT_Res turn_ball(TurnDir dir, AngleDeg turn_angle, float turnball_radius = CP_turnball_radius, KK_Mode kk_mode = KK_slow);
	KT_Res turn_ball(TurnDir dir, AngleDeg turn_angle, Command& command, float turnball_radius = CP_turnball_radius, KK_Mode kk_mode = KK_slow);
	KT_Res turn_ball(AngleDeg turn_angle, float turnball_radius = CP_turnball_radius, KK_Mode kk_mode = KK_slow);
	KT_Res turn_ball(AngleDeg turn_angle, Command& command, float turnball_radius = CP_turnball_radius, KK_Mode kk_mode = KK_slow);

/*	stop ball	*/
	bool stopball();
	bool stopball(Command& command);
/*	wisekick	*/
	KT_Res wisekick(float kickspeed, AngleDeg kickangle, KK_Mode kk_mode = KK_slow);
	KT_Res wisekick(float kickspeed, AngleDeg kickangle, Command& command, KK_Mode kk_mode = KK_slow);

	void CorrectForMyVel(float& kickspeed, float& kickangle);
	bool CanKickOut(Vector ballrelpos, float speed, float angle);	
};

#endif