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; } } }