int GpsController::NextMoveLogic (double distance, int angle){
int next_move;
if (CheckGoal()) {
cout << "Arrived at waypoint" << endl;
return 0;}
else{
if (angle < -11){
cout << "turn right" << endl;
return -15;
}
else if (angle > 11){
cout << "turn left" << endl;
return 15 ;
}
else{
if (distance > 100){
cout << "forward quickly" << endl;
return 9000;
if (distance < 0){
cout << "Distance was not calculated" << distance << endl;
return 0;
}
}
else if (distance < 100){
cout << "forward slowly" << endl;
return 9000;
}
else
cout << "Error in NextMoveLogic: " << distance << endl;
}
}
}
vector<Node*> Pendulum::FindPathIterations(int iterations) {
cout << "starting path iteration!\n";
vector<Node*> goal_nodes;
double min_cost = DBL_MAX;
int sol = 0;
for (int iter = 0; iter < iterations; iter++){
if(iter%5000==0){cout<<"iterations:"<<iter<<endl;}
//cout << sol << endl;
/**/
Node* prev = GetNodeToExpandRRT();
double u =((double)rand() / RAND_MAX - 0.5)*max_u;
Node* n = d.update(prev, u);
/*Test-Node generation
double testx = 2;
double testtheta = .1;
vector<double> times;
times.push_back(0);
times.push_back(0);
state_type s1;
s1.push_back(3);
s1.push_back(0);
s1.push_back(0);
s1.push_back(0);
state_type s2;
s2.push_back(40);
s2.push_back(0);
s2.push_back(2*PI-0.1);
s2.push_back(0);
vector<state_type> trajectory;
trajectory.push_back(s1);
trajectory.push_back(s2);
Node* n = new Node(0, testx, 0, testtheta, 0, 0, 0, NULL, times, trajectory);
n->print_node();
cout << Feasible(n) << endl;
*/
/**/
while (!Feasible(n)){
//n->print_node();
//cout << "found unfeasible node!\n";
int baditer = 1;
if(baditer%5000==0){cout<<"bad iterations:"<<baditer<<endl;}
delete n;
prev = GetNodeToExpandRRT();
u =((double)rand() / RAND_MAX - 0.5)*max_u;
n = d.update(prev, u);
}
prev->add_child(n);
//n->print_node();
if (CheckGoal(n)){
if (n->cost < min_cost){
min_cost = n->cost;
goal_nodes.push_back(n);
cout << "found better goal node! " << n->cost << endl;
sol = sol+1;
} /*
else{
cout << "found less optimal goal node! " << n->cost << endl;
}
*/
//Time and work are strictly positive
} else {
double pt[4] = {n->x*10.0/(bounds->ux-bounds->lx), n->v*10.0/(2*bounds->max_v)+5.0, n->theta*10.0/(6.28318530718), n->w*10.0/(2*bounds->max_w)+5.0};
if (kd_insert(nodes_tree, pt, n) != 0) {
cout << "didn't insert root successfully\n";
}
nodes.push_back(n);
}
}
cout << "finished iterating!\n";
cout << "Number of solutions found: " << sol << endl;
#ifdef OUTNEAREST
if (sol < 1){
double pt[4] = {b->x*10.0/(bounds->ux-bounds->lx), b->v*10.0/(2*bounds->max_v)+5.0, b->theta*10.0/(6.28318530718), b->w*10.0/(2*bounds->max_w)+5.0};
goalnodes.push_back()
}
/* Applies changes(transformations) to objects and
* the method is called every frame in main. Therefore
* contributes to the effect of animation. */
void Game::Run(){
Point2d next_position;
float distance, nextx, nexty;
bool reset = false;
SpectatorsCheers();
switch (state){
case (HOLD):
if (score1 == 3 || score2 == 3)
// We use a bool and not a state because
// we want the game to be restarted when
// the player hits the ball again.
m_restart = true;
break;
case (GOAL):
case (WALK): {
// In case m_restart is true we must restart the game
if (m_restart){
state = RESTART;
m_restart = false;
break;
}
// Move if in a lower section
// We check if somebody scored a goal
if (state == WALK){
m_last_scored = CheckGoal(true);
if (m_last_scored != -1){
DrawScoreForTeam(m_last_scored);
reset = CheckGoalLimit();
if (reset == true) {
// Might be that the ball need to be
// reseted immediately after it crossed the
// line of the gate. That can happend if
// the speed is to high
BeginSpectatorsCheers();
PickNewRandomPlayer(!m_last_scored);
}
state = GOAL;
}
else {
// If the ball hits the wall it will bounce off
WallHitCheck();
}
}
else if (state == GOAL){
reset = CheckGoalLimit();
// If the ball reached the limit withing the goal
// then we give it to a player from the other team
if (reset == true){
BeginSpectatorsCheers();
PickNewRandomPlayer(!m_last_scored);
state = HOLD;
}
}
// We check for collisions with other players
nextx = SPEED * deltax * m_dt;
nexty = SPEED * deltay * m_dt;
next_position.set(ball->GetObject()->axiscenter.x + nextx,
ball->GetObject()->axiscenter.y + nexty);
std::vector<Point2d>::iterator it;
// with players from team1
for (it = player1_pos.begin(); it != player1_pos.end(); ++it){
distance = CheckCollision(next_position, *it);
if (distance > 0.0f){
m_team = 0;
m_player = it - player1_pos.begin();
if (m_player != m_player_last || m_team != m_team_last){
state = HOLD;
}
}
}
// with players from team2
for (it = player2_pos.begin(); it != player2_pos.end(); ++it){
distance = CheckCollision(next_position, *it);
if (distance > 0.0f){
m_team = 1;
m_player = it - player2_pos.begin();
if (m_player != m_player_last || m_team != m_team_last){
state = HOLD;
}
}
}
// In the case the ball has been thrown then
// we translate it on the field
m_system->objectTranslate(ball->GetObject(), nextx, nexty);
break;
}
case (BLANK):{
// We are going to wait
Sleep(3000);
// And then we are going to start the game again
Init();
state = HOLD;
break;
}
case (RESTART):{
field->RemoveScoreLines();
PickNewRandomPlayer(rand() % 2);
// We need to wait for the next frame to see the
// changes on the screen. Therefore we use another
// state.
state = BLANK;
m_restart = false;
ResetScore();
m_system->objectRemoveAll();
player1_pos.clear();
player2_pos.clear();
player1.clear();
player2.clear();
break;
}
}
}
