std::vector<glm::vec3> getConvexHull(std::vector<glm::vec3> &pts){
std::vector<glm::vec3> hull;
glm::vec3 h1,h2,h3;
if (pts.size() < 3) {
std::cout << "Error: you need at least three points to calculate the convex hull" << std::endl;
return hull;
}
std::sort(pts.begin(), pts.end(), &lexicalComparison);
hull.push_back(pts.at(0));
hull.push_back(pts.at(1));
int currentPoint = 2;
int direction = 1;
for (int i=0; i<3000; i++) { //max 1000 tries
hull.push_back(pts.at(currentPoint));
// look at the turn direction in the last three points
h1 = hull.at(hull.size()-3);
h2 = hull.at(hull.size()-2);
h3 = hull.at(hull.size()-1);
// while there are more than two points in the hull
// and the last three points do not make a right turn
while (!isRightTurn(h1, h2, h3) && hull.size() > 2) {
// remove the middle of the last three points
hull.erase(hull.end() - 2);
if (hull.size() >= 3) {
h1 = hull.at(hull.size()-3);
}
h2 = hull.at(hull.size()-2);
h3 = hull.at(hull.size()-1);
}
// going through left-to-right calculates the top hull
// when we get to the end, we reverse direction
// and go back again right-to-left to calculate the bottom hull
if (currentPoint == pts.size() -1 || currentPoint == 0) {
direction = direction * -1;
}
currentPoint+=direction;
if (hull.front()==hull.back()) {
if(currentPoint == 3 && direction == 1){
currentPoint = 4;
} else {
break;
}
}
}
return hull;
}
int TicTacToe::play(bool _xTurn, int pos) {
bool win;
if(!isRightTurn(_xTurn)) {
return INVALID_TURN;
}
if(!isValidMove(pos)) {
return INVALID_MOVE;
}
placeMove(_xTurn,pos);
plotGame();
win = won(_xTurn);
xTurn = !xTurn;
if(win) {
resetGame();
return WIN;
}
else if(isFull()){
resetGame();
return OVER;
}
else {
return NO_WIN;
}
}
//vector<ofPoint>
ofPolyline ofxGetConvexHull(vector<ofPoint> points) {
if (points.size()<3) return ofPolyline();
ofPoint h1,h2,h3;
sort(points.begin(), points.end(), lexicalComparison);
vector<ofPoint> hull;
hull.push_back(points.at(0));
hull.push_back(points.at(1));
int currentPoint = 2;
int direction = 1;
for (int i=0; i<1000; i++) { //max 1000 tries
hull.push_back(points.at(currentPoint));
// look at the turn direction in the last three points
h1 = hull.at(hull.size()-3);
h2 = hull.at(hull.size()-2);
h3 = hull.at(hull.size()-1);
// while there are more than two points in the hull
// and the last three points do not make a right turn
while (!isRightTurn(h1, h2, h3) && hull.size() > 2) {
// remove the middle of the last three points
hull.erase(hull.end() - 2);
if (hull.size() >= 3) {
h1 = hull.at(hull.size()-3);
}
h2 = hull.at(hull.size()-2);
h3 = hull.at(hull.size()-1);
}
// going through left-to-right calculates the top hull
// when we get to the end, we reverse direction
// and go back again right-to-left to calculate the bottom hull
if (currentPoint == points.size() -1 || currentPoint == 0) {
direction = direction * -1;
}
currentPoint+= direction;
if (hull.front()==hull.back()) break;
}
return ofPolyline(hull);
}
std::vector<RouteStep> anticipateLaneChange(std::vector<RouteStep> steps,
const double min_duration_needed_for_lane_change)
{
// Lane anticipation works on contiguous ranges of quick steps that have lane information
const auto is_quick_has_lanes = [&](const RouteStep &step) {
const auto is_quick = step.duration < min_duration_needed_for_lane_change;
const auto has_lanes = step.intersections.front().lanes.lanes_in_turn > 0;
return has_lanes && is_quick;
};
using StepIter = decltype(steps)::iterator;
using StepIterRange = std::pair<StepIter, StepIter>;
std::vector<StepIterRange> quick_lanes_ranges;
const auto range_back_inserter = [&](StepIterRange range) {
if (std::distance(range.first, range.second) > 1)
quick_lanes_ranges.push_back(std::move(range));
};
util::group_by(begin(steps), end(steps), is_quick_has_lanes, range_back_inserter);
// The lanes for a keep straight depend on the next left/right turn. Tag them in advance.
std::unordered_set<const RouteStep *> is_straight_left;
std::unordered_set<const RouteStep *> is_straight_right;
// Walk backwards over all turns, constraining possible turn lanes.
// Later turn lanes constrain earlier ones: we have to anticipate lane changes.
const auto constrain_lanes = [&](const StepIterRange &turns) {
const std::reverse_iterator<StepIter> rev_first{turns.second};
const std::reverse_iterator<StepIter> rev_last{turns.first};
// We're walking backwards over all adjacent turns:
// the current turn lanes constrain the lanes we have to take in the previous turn.
util::for_each_pair(rev_first, rev_last, [&](RouteStep ¤t, RouteStep &previous) {
const auto current_inst = current.maneuver.instruction;
const auto current_lanes = current.intersections.front().lanes;
// Constrain the previous turn's lanes
auto &previous_lanes = previous.intersections.front().lanes;
const auto previous_inst = previous.maneuver.instruction;
// Lane mapping (N:M) from previous lanes (N) to current lanes (M), with:
// N > M, N > 1 fan-in situation, constrain N lanes to min(N,M) shared lanes
// otherwise nothing to constrain
const bool lanes_to_constrain = previous_lanes.lanes_in_turn > 1;
const bool lanes_fan_in = previous_lanes.lanes_in_turn > current_lanes.lanes_in_turn;
if (!lanes_to_constrain || !lanes_fan_in)
return;
// We do not have a mapping from lanes to lanes. All we have is the lanes in the turn
// and all the lanes at that situation. To perfectly handle lane anticipation in cases
// where lanes in the turn fan in but for example the overall lanes at that location
// fan out, we would have to know the asymmetric mapping of lanes. This is currently
// not possible at the moment. In the following we implement a heuristic instead.
const LaneID current_num_lanes_right_of_turn = current.numLanesToTheRight();
const LaneID current_num_lanes_left_of_turn = current.numLanesToTheLeft();
const LaneID num_shared_lanes = std::min(current_lanes.lanes_in_turn, //
previous_lanes.lanes_in_turn); //
// 0/ Tag keep straight with the next turn's direction if available
const auto previous_is_straight =
!isLeftTurn(previous_inst) && !isRightTurn(previous_inst);
if (previous_is_straight)
{
if (isLeftTurn(current_inst) || is_straight_left.count(¤t) > 0)
is_straight_left.insert(&previous);
else if (isRightTurn(current_inst) || is_straight_right.count(¤t) > 0)
is_straight_right.insert(&previous);
}
// 1/ How to anticipate left, right:
const auto anticipate_for_left_turn = [&] {
// Current turn is left turn, already keep left during previous turn.
// This implies constraining the rightmost lanes in previous step.
LaneID new_first_lane_from_the_right =
previous_lanes.first_lane_from_the_right // start from rightmost lane
+ previous_lanes.lanes_in_turn // one past leftmost lane
- num_shared_lanes; // back number of new lanes
// The leftmost target lanes might not be involved in the turn. Figure out
// how many lanes are to the left and not in the turn.
new_first_lane_from_the_right -=
std::min(current_num_lanes_left_of_turn, num_shared_lanes);
previous_lanes = {num_shared_lanes, new_first_lane_from_the_right};
};
const auto anticipate_for_right_turn = [&] {
// Current turn is right turn, already keep right during the previous turn.
// This implies constraining the leftmost lanes in the previous turn step.
LaneID new_first_lane_from_the_right = previous_lanes.first_lane_from_the_right;
// The rightmost target lanes might not be involved in the turn. Figure out
// how many lanes are to the right and not in the turn.
new_first_lane_from_the_right +=
std::min(current_num_lanes_right_of_turn, num_shared_lanes);
previous_lanes = {num_shared_lanes, new_first_lane_from_the_right};
};
// 2/ When to anticipate a left, right turn
if (isLeftTurn(current_inst))
anticipate_for_left_turn();
else if (isRightTurn(current_inst))
anticipate_for_right_turn();
else // keepStraight
{
// Heuristic: we do not have a from-lanes -> to-lanes mapping. What we use
// here instead in addition is the number of all lanes (not only the lanes
// in a turn):
//
// -v-v v-v- straight follows
// | | | |
// <- v v -> keep straight here
// | |
// <-| |->
//
// A route from the top left to the bottom right here goes over a keep
// straight. If we handle all keep straights as right turns (in right-sided
// driving), we wrongly guide the user to the rightmost lanes in the first turn.
// Not only is this wrong but the opposite of what we expect.
//
// The following implements a heuristic to determine a keep straight's
// direction in relation to the next step. In the above example we would get:
//
// coming from right, going to left (in direction of way) -> handle as left turn
if (is_straight_left.count(¤t) > 0)
anticipate_for_left_turn();
else if (is_straight_right.count(¤t) > 0)
anticipate_for_right_turn();
else // FIXME: right-sided driving
anticipate_for_right_turn();
}
// We might have constrained the previous step in a way that makes it compatible
// with the current step. If we did so we collapse it here and mark the current
// step as invalid, scheduled for later removal.
if (collapsable(previous, current))
{
previous = elongate(previous, current);
current.maneuver.instruction = TurnInstruction::NO_TURN();
}
});
};
std::for_each(begin(quick_lanes_ranges), end(quick_lanes_ranges), constrain_lanes);
// Lane Anticipation might have collapsed steps after constraining lanes. Remove invalid steps.
steps = removeNoTurnInstructions(std::move(steps));
return steps;
}
std::vector<RouteStep> anticipateLaneChange(std::vector<RouteStep> steps,
const double min_duration_needed_for_lane_change)
{
// Postprocessing does not strictly guarantee for only turns
const auto is_turn = [](const RouteStep &step) {
return step.maneuver.instruction.type != TurnType::NewName &&
step.maneuver.instruction.type != TurnType::Notification;
};
const auto is_quick = [min_duration_needed_for_lane_change](const RouteStep &step) {
return step.duration < min_duration_needed_for_lane_change;
};
const auto is_quick_turn = [&](const RouteStep &step) {
return is_turn(step) && is_quick(step);
};
// Determine range of subsequent quick turns, candidates for possible lane anticipation
using StepIter = decltype(steps)::iterator;
using StepIterRange = std::pair<StepIter, StepIter>;
std::vector<StepIterRange> subsequent_quick_turns;
const auto keep_turn_range = [&](StepIterRange range) {
if (std::distance(range.first, range.second) > 1)
subsequent_quick_turns.push_back(std::move(range));
};
util::group_by(begin(steps), end(steps), is_quick_turn, keep_turn_range);
// Walk backwards over all turns, constraining possible turn lanes.
// Later turn lanes constrain earlier ones: we have to anticipate lane changes.
const auto constrain_lanes = [](const StepIterRange &turns) {
const std::reverse_iterator<StepIter> rev_first{turns.second};
const std::reverse_iterator<StepIter> rev_last{turns.first};
// We're walking backwards over all adjacent turns:
// the current turn lanes constrain the lanes we have to take in the previous turn.
util::for_each_pair(rev_first, rev_last, [](RouteStep ¤t, RouteStep &previous) {
const auto current_inst = current.maneuver.instruction;
const auto current_lanes = current.intersections.front().lanes;
// Constrain the previous turn's lanes
auto &previous_lanes = previous.intersections.front().lanes;
// Lane mapping (N:M) from previous lanes (N) to current lanes (M), with:
// N > M, N > 1 fan-in situation, constrain N lanes to min(N,M) shared lanes
// otherwise nothing to constrain
const bool lanes_to_constrain = previous_lanes.lanes_in_turn > 1;
const bool lanes_fan_in = previous_lanes.lanes_in_turn > current_lanes.lanes_in_turn;
if (!lanes_to_constrain || !lanes_fan_in)
return;
// In case there is no lane information we work with one artificial lane
const auto current_adjusted_lanes = std::max(current_lanes.lanes_in_turn, LaneID{1});
const auto num_shared_lanes = std::min(current_adjusted_lanes, //
previous_lanes.lanes_in_turn);
if (isRightTurn(current_inst))
{
// Current turn is right turn, already keep right during the previous turn.
// This implies constraining the leftmost lanes in the previous turn step.
previous_lanes = {num_shared_lanes, previous_lanes.first_lane_from_the_right};
}
else if (isLeftTurn(current_inst))
{
// Current turn is left turn, already keep left during previous turn.
// This implies constraining the rightmost lanes in the previous turn step.
const LaneID shared_lane_delta = previous_lanes.lanes_in_turn - num_shared_lanes;
const LaneID previous_adjusted_lanes =
std::min(current_adjusted_lanes, shared_lane_delta);
const LaneID constraint_first_lane_from_the_right =
previous_lanes.first_lane_from_the_right + previous_adjusted_lanes;
previous_lanes = {num_shared_lanes, constraint_first_lane_from_the_right};
}
});
};
std::for_each(begin(subsequent_quick_turns), end(subsequent_quick_turns), constrain_lanes);
return steps;
}
