[Core] tidy ballspeedmodel

Core/src/Algorithm/BallSpeedModel.cpp

@@ -1,4 +1,5 @@
 #include "BallSpeedModel.h"
+#include "VisionModule.h"
 #include "staticparams.h"
 #include <cmath>
 #include <string>
@@ -8,122 +9,53 @@
 using namespace std;
 
 namespace{
-	static int FRICTION;
+	// _DEC > 0 from friction
+	static double _DEC;
 }
-static double oneFromFive(const double x1,const double y1,const double x2,const double y2,const double x3);
 
 CBallSpeedModel::CBallSpeedModel():_ballVel(0,0),_ballPos(-9999,-9999){
     bool IS_SIMULATION;
     ZSS::ZParamManager::instance()->loadParam(IS_SIMULATION,"Alert/IsSimulation",false);
     if (IS_SIMULATION)
-        ZSS::ZParamManager::instance()->loadParam(FRICTION,"AlertParam/Friction4Sim",800);
-    else
-        ZSS::ZParamManager::instance()->loadParam(FRICTION,"AlertParam/Friction4Real",1520);
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Sim", 1200);
+	else
+		ZSS::ZParamManager::instance()->loadParam(_DEC, "AlertParam/BallDec_Real", 400);
 }
 CBallSpeedModel::~CBallSpeedModel(){
 }
-
-//todo
-CVector CBallSpeedModel::speedForTime(double frame, const CVisionModule* pVision ){
-	update(pVision);
-    return speedForTime_FM(frame);
-}
-
-//todo
-CVector CBallSpeedModel::speedForDist(double dist, const CVisionModule* pVision) {
-    update(pVision);
-    return speedForDist_FM(dist);
+double CBallSpeedModel::timeForDist(const double dist){
+	return std::get<0>(predictForDist(dist));
 }
-
-//use new speed mode
-double CBallSpeedModel::timeForDist(double dist, const CVisionModule* pVision ){
-    update(pVision);
-    return timeForDist_FM(dist);
-}
-
-//use new speed mode
-CGeoPoint CBallSpeedModel::posForTime(double frame, const CVisionModule* pVision ){
-    update(pVision);
-    return posForTime_FM(frame);
+CVector CBallSpeedModel::speedForDist(const double dist){
+	return std::get<1>(predictForDist(dist));
 }
-CVector CBallSpeedModel::speedForTime_FM(double frame) {
-	auto vel = _ballVel.mod() - 0.5 * FRICTION * frame*1.0 / PARAM::Vision::FRAME_RATE;
-	if (vel < 0) vel = 0;
-	return _ballVel / _ballVel.mod() * vel;
-}
-CVector CBallSpeedModel::speedForDist_FM(double dist) {
-	auto v = _ballVel.mod();
-	auto t = v*v - FRICTION*dist;
-	if (t <= 0) return CVector(0, 0);
-	auto vel = std::sqrt(t);
-	return _ballVel / _ballVel.mod() * vel;
-}
-double CBallSpeedModel::timeForDist_FM(double dist) {
-	auto v = _ballVel.mod();
-	auto a = 0.25*FRICTION;
-	auto b = -v;
-	auto c = dist;
-	auto delta = b*b - 4 * a*c;
-	if (delta < 0) return -1;
-    return 1.0 / (2 * a)*(-b - std::sqrt(delta));
-}
-CGeoPoint CBallSpeedModel::posForTime_FM(double frame) {
+std::tuple<double, CVector> CBallSpeedModel::predictForDist(const double dist){
+	this->update();
+	auto maxDist = _ballVel.mod() * _ballVel.mod() / (2 * _DEC);
+	if (dist > maxDist) {
+		return {-1, CVector(0,0)};
+	}
 	auto v0 = _ballVel.mod();
-	auto v1 = v0 - 2 * FRICTION*frame/PARAM::Vision::FRAME_RATE;
-	double d;
-	if (v1 < 0)
-		d = v0*v0 / FRICTION;
-	else
-		d = (v0 + v1)*frame / PARAM::Vision::FRAME_RATE / 2;
-	return _ballPos + (_ballVel / _ballVel.mod()*d);
+	auto v1 = sqrt(v0 * v0 - 2 * _DEC * dist);
+	auto time = (v0 - v1) / _DEC;
+	return {time, Utils::Polar2Vector(v1, _ballVel.dir())};
 }
-//todo
-//double CBallSpeedModel::CalKickInitSpeed(const double dist)
-//{
-//	double vt = 100;
-//	if (dist < 100) {
-//		vt = 120;
-//	} else if (dist >= 100 && dist < 200) {
-//		vt = 200;
-//	} else if (dist >= 200 && dist < 300) {
-//		vt = 350;
-//	} else if (dist >= 300 && dist < 400) {
-//		vt = 350;
-//	} else if (dist >= 400) {
-//		vt = 350;
-//	}
-//	vt = vt+100;//让传球速度变小一点
-//	double ballVO = sqrt(vt * vt + 2 * 20 * dist);
-//	//迭代法计算初速度
-//	for (int i = 1;i<=3;i++){
-//		if(ballVO > 240){
-//			cal_acc = 10;
-//		} else if(ballVO > 190 && ballVO <= 240){
-//			cal_acc = ( 0.0025 * ballVO + 0.18 ) * 60;
-//		} else if(ballVO > 140 && ballVO <= 190){
-//			cal_acc = ( (0.0025 + ( 190 - ballVO) *0.00001)  * ballVO + 0.18 ) * 60;
-//		} else if (ballVO > 10 && ballVO <= 140){
-//			cal_acc = ( 0.003  * ballVO + 0.18 ) * 60;
-//		}
-//		ballVO = sqrt(vt * vt + 2 * cal_acc * dist);
-//	}
-//	return ballVO;
-//}
-void CBallSpeedModel::update( const CVisionModule* pVision ){
-	//lastBallPos = _ballPos;
-    _ballPos = pVision->ball().Valid() ? pVision->ball().RawPos() : pVision->ball().Pos();
-    _ballVel = pVision->ball().Vel();
+MobileVisionT CBallSpeedModel::poseForTime(const double time){
+	this->update();
+	MobileVisionT predictPose;
+	auto v0 = _ballVel.mod();
+	auto v1 = std::max(v0 - _DEC * time, 0.0);
+	auto runtime = std::min(v0 / _DEC, time);
+	double dist = (v0 + v1) * runtime / 2;
+	predictPose.SetPos(_ballPos + Utils::Polar2Vector(dist, _ballVel.dir()));
+	predictPose.SetVel(Utils::Polar2Vector(v1, _ballVel.dir()));
+	return predictPose;
 }
 
-//double CBallSpeedModel::getAcc(double speed) const{
-//	if(speed >= 500) return 1.7;
-//	if(speed > 300) return speed/200.0 - 0.8;	//300 - 500 line : (300,0.7) , (500,1.7)
-//	if(speed >= 0.7) return 0.7;
-//	if(speed >= 0) return speed;
-//	cout << "BallSpeedModel.cpp : Speed Mod < 0 in getAcc Function" << endl;
-//	return -1;
-//}
+void CBallSpeedModel::update(){
+	if(lastCycle == vision->getCycle()) return;
+	lastCycle = vision->getCycle();
 
-//static double oneFromFive(const double x1,const double y1,const double x2,const double y2,const double x3){
-//	return (y2-y1)/(x2-x1)*(x3-x2)+y2;
-//}
+	_ballPos = vision->ball().Valid() ? vision->ball().RawPos() : vision->ball().Pos();
+	_ballVel = vision->ball().Vel();
+}

Core/src/Algorithm/BallSpeedModel.h

@@ -1,43 +1,26 @@
 #ifndef _BALL_SPEED_MODEL_H_
 #define _BALL_SPEED_MODEL_H_
+#include <tuple>
+#include "singleton.hpp"
+#include "WorldDefine.h"
 
-#include "VisionModule.h"
-
-class CBallSpeedModel{
+class CVisionModule;
+class CBallSpeedModel
+{
 public:
 	CBallSpeedModel();
 	~CBallSpeedModel();
-	CVector speedForTime(double frame, const CVisionModule* pVision);   //计算若干帧以后的速度
-	CVector speedForDist(double dist, const CVisionModule* pVision); //计算若干距离后的速度
-	double timeForDist(double dist, const CVisionModule* pVision);      //计算球运动若干距离的时间
-	CGeoPoint posForTime(double frame, const CVisionModule* pVision);   //计算若干帧以后的绝对位置
-	void update( const CVisionModule* pVision );
-//	double CalKickInitSpeed(const double dist);
-private:
-    CVector speedForTime_FM(double frame);   //计算若干帧以后的速度
-    CVector speedForDist_FM(double dist); //计算若干距离后的速度
-    double timeForDist_FM(double dist);      //计算球运动若干距离的时间
-    CGeoPoint posForTime_FM(double frame);   //计算若干帧以后的绝对位置
+	inline void registerVision(CVisionModule *vision) { this->vision = vision; }
+	std::tuple<double,CVector> predictForDist(const double dist);	// 计算若干距离后的速度
+	double timeForDist(const double dist);      //计算球运动若干距离的时间
+	CVector speedForDist(const double dist);	// 计算球运动若干距离的速度
+	MobileVisionT poseForTime(const double time); // 计算若干帧以后的绝对位置
 private:
-	CVector _ballVel;
-//	CVector _lastBallVel;
+	void update();
 	CGeoPoint _ballPos;
-//	double cal_acc;
-//	double cal_acc_high_speed;
-//	double cal_acc_low_speed;
-//	double _last_dist;
-private:
-	//add some private function to calculate ACC --hzy 2016.6.12 test
-	//double getAcc(double speed) const;
-//	const bool readTableFile_CheckTable(const string& filename);
-//	const double timeForDist_CheckTable(const double dist) const;
-//	const double distForTime_CheckTable(const int frame) const;
-//	const double checkTime_CheckTable(const double const* _array,const double data) const;
-//	const double checkData_CheckTable(const double const* _array,const double time) const;
-//	double *_speed_data;
-//	double *_dist_data;
-//	int _num;
+	CVector _ballVel;
+	int lastCycle = 0;
+	CVisionModule *vision = nullptr;
 };
 typedef Singleton< CBallSpeedModel > BallSpeedModel;
 #endif // _BALL_SPEED_MODEL_H_
-

Core/src/LuaModule/ballspeedmodel.pkg

@@ -0,0 +1,7 @@
+$#include <WorldDefine.h>
+class CBallSpeedModel
+{
+	double timeForDist(const double dist);
+	CVector speedForDist(const double dist);
+	MobileVisionT poseForTime(const double time);
+};

Core/src/LuaModule/global.pkg

@@ -5,4 +5,5 @@ CKickStatus*    kickStatus;
 CDribbleStatus* dribbleStatus;
 CGDebugEngine*  debugEngine;
 CWorldModel*	world;
-CSkillAPI*      skillapi;
+CSkillAPI*      skillapi;
+CBallSpeedModel* ballModel;

Core/src/LuaModule/zeus.pkg

@@ -10,4 +10,5 @@ $pfile "gamestate.pkg"
 $pfile "dribblestatus.pkg"
 $pfile "utils.pkg"
 $pfile "munkres.pkg"
-$pfile "skillapi.pkg"
+$pfile "skillapi.pkg"
+$pfile "ballspeedmodel.pkg"

Core/src/Main/Global.cpp

@@ -6,6 +6,7 @@ CDribbleStatus* dribbleStatus;
 CGDebugEngine*  debugEngine;
 CWorldModel* world;
 CSkillAPI* skillapi;
+CBallSpeedModel *ballModel;
 void initializeSingleton()
 {
 	vision        = VisionModule::Instance();
@@ -14,4 +15,5 @@ void initializeSingleton()
     debugEngine   = GDebugEngine::Instance();
     world         = WorldModel::Instance();
 	skillapi	  = SkillAPI::Instance();
+	ballModel = BallSpeedModel::Instance();
 }

Core/src/Main/Global.h

@@ -8,12 +8,14 @@
 #include "singleton.hpp"
 #include "WorldModel.h"
 #include "skillapi.h"
+#include "BallSpeedModel.h"
 
 extern CVisionModule*  vision;
 extern CKickStatus*    kickStatus;
 extern CDribbleStatus* dribbleStatus;
 extern CGDebugEngine*  debugEngine;
 extern CWorldModel* world;
 extern CSkillAPI* skillapi;
+extern CBallSpeedModel* ballModel;
 void initializeSingleton();
 #endif

Core/src/Main/zeus_main.cpp

@@ -60,6 +60,7 @@ int runLoop() {
     CCommandInterface::instance(option);
     vision->registerOption(option);
     vision->startReceiveThread();
+    ballModel->registerVision(vision);
     skillapi->registerVision(vision);
     decision = new CDecisionModule(vision);
     action = new CActionModule(vision, decision);

ZBin/lua_scripts/worldmodel/param.lua

@@ -32,7 +32,7 @@ playerFrontToCenter = 76
 lengthRatio	= 1.5
 widthRatio	= 1.5
 stopRatio = 1.1
-frameRate = 75
+frameRate = 73
 --~ -------------------------------------------
 --~ used for debug
 --~ -------------------------------------------