#define _WINSOCK_DEPRECATED_NO_WARNINGS
#include <iomanip> //////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <WINSOCK2.H>
#include "Ws2tcpip.h"
#include "Winsock2.h"
#include "zss_cmd.pb.h"
#include "vision_detection.pb.h"
#include "zss_debug.pb.h"
#include <random>
#include <ctime>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cstdio>
#include <cstdlib>
using std::cin;
using std::cout;
using std::cerr;
using std::endl;
double ROBO[30][2];
int NumPoint = 0, IniSizeBlue,Score=-1;
double cost_path[500001];//500*500的地图共250000个点，留有冗余
double rrt_point[10000][2];
#define PI 3.14159265
#define ME 0
#define MAX_V 300
#define mid_JS_range 100
#define final_JS_range 120
double dis_lev = 0;
int Count = 0;
double D_heading[2000];
Vision_DetectionFrame frame;
Vision_DetectionBall balls;
Vision_DetectionRobot robots_yellow;
void getROBO()
{
for (int ii = 0; ii < frame.robots_blue_size(); ++ii)
{
int i = frame.robots_blue(ii).robot_id();
//cout << "RBid= " << i << endl;
Vision_DetectionRobot robots_blue = frame.robots_blue(ii);
ROBO[i][1] = robots_blue.x() / 10.0 + 300;
ROBO[i][0] = 450 - (robots_blue.y() / 10.0 + 225);
//cout << ROBO[i][0] << " " << ROBO[i][1] << endl;
//cout << /*robots_blue.raw_vel_x() << robots_blue.raw_vel_y() << */robots_blue.orientation() << endl;
}
for (int ii = 0; ii < frame.robots_yellow_size(); ++ii)
{
int i = frame.robots_yellow(ii).robot_id();
//cout << "RBid= " << i << endl;
Vision_DetectionRobot robots_yellow = frame.robots_yellow(ii);
ROBO[i + 16][1] = robots_yellow.x() / 10.0 + 300;
ROBO[i + 16][0] = 450 - (robots_yellow.y() / 10.0 + 225);
//cout << ROBO[i][0] << " " << ROBO[i][1] << endl;
}
}
double barrier_x[35];
double barrier_y[35];
double barrier_angle[35];
double distance[35];
double barrier_V[35];
double board_angle[8];
double board_dis[8];
double board_V[8];
void getBarrier()
{
for (int i = 0; i <= 31; ++i) barrier_x[i] = barrier_y[i] = -1;
for (int ii = 0; ii < frame.robots_blue_size(); ++ii)
{
int i = frame.robots_blue(ii).robot_id();
//cout << "RBid= " << i << endl;
barrier_x[i] = frame.robots_blue(ii).x() / 10;
barrier_y[i] = -frame.robots_blue(ii).y() / 10;
//cout << barrier_x[i] << " " << barrier_y[i] << endl;
}
for (int ii = 0; ii < frame.robots_yellow_size(); ++ii)
{
int i = frame.robots_yellow(ii).robot_id();
//cout << "RBid= " << i << endl;
barrier_x[i + 16] = frame.robots_yellow(ii).x() / 10;
barrier_y[i + 16] = -frame.robots_yellow(ii).y() / 10;
//cout << barrier_x[i+8] << " " << barrier_y[i+8] << endl;
}
}
double MAX(double a, double b)
{
return (a > b) ? a : b;
}
struct point
{
int parent;
double pos_x, pos_y;
};
int getIndex(double x, double y)
{
return int(int(x - 1) * 600 + int(y));
}
char* getIP()
{
BYTE minorVer = 2;
BYTE majorVer = 2;
WSADATA wsaData;
WORD sockVersion = MAKEWORD(minorVer, majorVer);
WSAStartup(sockVersion, &wsaData);
int i;
char szHost[256];
gethostname(szHost, 256);
hostent *pHost = gethostbyname(szHost);
in_addr addr;
char *p = pHost->h_addr_list[0];
memcpy(&addr.S_un.S_addr, p, pHost->h_length);
char *szIp = inet_ntoa(addr);
return (szIp);
}
double measure_distance(point x, point y)
{
return sqrt(pow(fabs(x.pos_x - y.pos_x), 2) + pow(fabs(x.pos_y - y.pos_y), 2));
}
int WALL(double x, double y)
{
for (int i = 0; i <= 31; ++i)
if ((ROBO[i][0] != -1 || ROBO[i][1] != -1) && i != ME)
if (sqrt(pow(fabs(x - ROBO[i][0]), 2) + pow(fabs(y - ROBO[i][1]), 2)) < 32.0)
return 1;
return 0;
}
int argmin(std::vector<point>& seeds, point seed)
{
int result = 0;
double minimun = 1.0e10;
for (int i = 0; i < seeds.size(); i++)
{
double dis = measure_distance(seeds[i], seed);
if (dis < minimun)
{
result = i;
minimun = dis;
}
}
return result;
}
int detect(point a, point b)
{
double deltx = (b.pos_x - a.pos_x) / 100;
double delty = (b.pos_y - a.pos_y) / 100;
for (double i = 1; i <= 100; i += 1.0) {
if (WALL(a.pos_x + i * deltx, a.pos_y + i * delty) > 0) return 1;
}
return 0;
}
#pragma comment(lib, "ws2_32.lib")
double Measure_Dis(double x, double y, double x1, double Y1)
{
return (sqrt(pow(x - x1, 2) + pow(y - Y1, 2)));
}
bool has_arrive(double x, double y, double x1, double y1, double dis_lev) {
if (sqrt(pow(x - x1, 2) + pow(y - y1, 2)) < dis_lev)
return 1;
else
return 0;
}
double compute_heading(double x, double y, double head_x, double head_y, double theta) {
double bias = 0.0;
if ((head_x - x) < 0.0) bias = 3.14159265;
//printf(" jiajiao: %.4lf\n", atan((head_y - y) / (head_x - x))+bias);
return atan((head_y - y) / (head_x - x)) + bias - theta;
}
double compute_angle(double x, double y, double headx, double heady) {
double bias = 0.0;
if ((headx - x) < 0.0) bias = 3.14159265;
//printf(" jiajiao: %.4lf\n", atan((head_y - y) / (head_x - x))+bias);
return atan((heady - y) / (headx - x)) + bias;
}
int SOCKADDR_IN_SIZE = sizeof(SOCKADDR_IN);
const u_short DEFAULT_PORT = 23333;
const size_t MSG_BUF_SIZE = 2048;
const size_t IP_BUF_SIZE = 256;
//const int Port = 50001;
int RRTstar(double start_x, double start_y, double end_x, double end_y, double beta)
{
int xs_ = 450, ys_ = 600;
int ns_ = xs_ * ys_;
int lethal_cost_ = 253; //barrier height
double step = 25.0; //调节每步最小步长
std::vector<point> seeds; //随机生成点的集合
point end_point; //point struct记录其xy坐标与父结点在seeds中的下标
end_point.pos_x = end_x;
end_point.pos_y = end_y;
point current;
current.pos_x = start_x;
current.pos_y = start_y;
current.parent = -1; //出发点的父结点为源点-1
//cout << "SXSY EXEY ";
//cout << current.pos_x << " " << current.pos_y << " " << end_point.pos_x << " " << end_point.pos_y; // set x and set y
int Xmin;
seeds.push_back(current); //加入vector，进行广搜
int cycles = 0;
//for (int i = 0; i < 500000; ++i) cost_path[i] = 5000;
cost_path[getIndex(current.pos_x, current.pos_y)] = 0;//除起点外距离均为max，max为防止累加导致溢出不宜过大，如0x3f3f3f3，本地图5000足够大。
//std::default_random_engine generator(time(NULL));//依系统时间为种子生成随机数
clock_t st; st = clock();
int Able = 0;
while (cycles < 5000)
{
cycles++;
//std::uniform_int_distribution<int> random_gen_y(0, 600);
//std::uniform_int_distribution<int> random_gen_x(0, 450);
int ran_x = rand() % 400 + 25;
int ran_y = rand() % 550 + 25;
current.pos_x = ran_x;
current.pos_y = ran_y; //X.current<-X.rand
int nearest = argmin(seeds, current); //get X.nearest
double minimum = measure_distance(seeds[nearest], current);
if (measure_distance(seeds[nearest], current) < step)
{
cycles--; //离X.nearest距离不足最小步长，则重新随机取点
continue;
}
else {
double a = 1.0;//前往随机采样点方向的增长系数
double distance_x_end = end_point.pos_x - seeds[nearest].pos_x;
double distance_y_end = end_point.pos_y - seeds[nearest].pos_y;
double delta_x = current.pos_x - seeds[nearest].pos_x;
double delta_y = current.pos_y - seeds[nearest].pos_y;
double rate = sqrt(pow(step, 2) / (pow(fabs(delta_x), 2) + pow(fabs(delta_y), 2)));
double rate_end = sqrt(pow(step, 2) / (pow(fabs(distance_x_end), 2) + pow(fabs(distance_y_end), 2)));
//以线性的步长/直线距离为扩展率，离得越近“拉力”越大
double forward_x = seeds[nearest].pos_x + delta_x * rate * a + distance_x_end * rate_end * beta;
double forward_y = seeds[nearest].pos_y + delta_y * rate * a + distance_y_end * rate_end * beta;
//依系数加权合成X.new位置
if (WALL(forward_x, forward_y) > 0)
{
//发现目标点撞墙
//cout << "shit" << forward_x << " " << forward_y << endl;
double forward_x_again = seeds[nearest].pos_x + delta_x * rate * a;
do
