int majorityElement(vector<int>& nums) {
int candidateIndx = findCandidate(nums);
// no need to verify, as there is obvoiously one majority element
return nums[candidateIndx];
}
int majorityElement(vector<int>& num){
if(num.size() < 1) return -1;
int candidate = findCandidate(num);
if(isMajority(num, candidate)){
return candidate;
}else{
return - 1;
}
}
bool
MSLaneChangerSublane::change() {
// variant of change() for the sublane case
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gDebugSelectedVehicle == vehicle->getID()) {
int bla = 0;
}
#endif
assert(vehicle->getLane() == (*myCandi).lane);
assert(!vehicle->getLaneChangeModel().isChangingLanes());
#ifndef NO_TRACI
if (vehicle->isRemoteControlled()) {
return false; // !!! temporary; just because it broke, here
}
#endif
vehicle->updateBestLanes(); // needed?
for (int i = 0; i < (int) myChanger.size(); ++i) {
vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
}
// update expected speeds
int sublaneIndex = 0;
for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
vehicle->getLaneChangeModel().updateExpectedSublaneSpeeds(ce->ahead, sublaneIndex, ce->lane->getIndex());
sublaneIndex += ce->ahead.numSublanes();
}
StateAndDist right = checkChangeHelper(vehicle, -1);
StateAndDist left = checkChangeHelper(vehicle, 1);
StateAndDist current = checkChangeHelper(vehicle, 0);
StateAndDist decision = vehicle->getLaneChangeModel().decideDirection(current,
vehicle->getLaneChangeModel().decideDirection(right, left));
if ((decision.state & LCA_WANTS_LANECHANGE) != 0 && (decision.state & LCA_BLOCKED) == 0) {
// change if the vehicle wants to and is allowed to change
if (vehicle->getLaneChangeModel().debugVehicle()) {
std::cout << SIMTIME << " decision=" << toString((LaneChangeAction)decision.state) << " latDist=" << decision.latDist << "\n";
}
vehicle->getLaneChangeModel().setOwnState(decision.state);
return startChangeSublane(vehicle, myCandi, decision.latDist);
}
if ((right.state & (LCA_URGENT)) != 0 && (left.state & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
left.state = 0;
}
vehicle->getLaneChangeModel().setOwnState(right.state | left.state | current.state);
registerUnchanged(vehicle);
return false;
}
/* Function to print Majority Element */
void printMajority(int * array, int size)
{
/* Find the candidate for Majority*/
int cand = findCandidate(array, size);
/* Print the candidate if it is Majority*/
if(isMajority(array, size, cand))
printf(" %d ", cand);
else
printf("NO Majority Element");
}
/* Function to print Majority Element */
void printMajority(int a[], int size)
{
/* Find the candidate for Majority*/
int cand = findCandidate(a, size);
/* Print the candidate if it is Majority*/
if(isMajority(a, size, cand)){
*(dram_addr + index_ram) = 0x1 | cand;
index_ram++;
}
else{
*(dram_addr + index_ram) = cand;
index_ram++;
}
}
/*!
* \brief Solves the given task.
* \param numCities Number of cities in the task.
* \param task The matrix of city-to-city travel costs.
* \return Pointer to the root of the solution tree.
*
* \todo TODO: Comment the algorithm.
*/
SStep *CTSPSolver::solve(int numCities, const TMatrix &task)
{
if (numCities < 3)
return NULL;
QMutexLocker locker(&mutex);
cleanup();
canceled = false;
locker.unlock();
nCities = numCities;
SStep *step = new SStep();
step->matrix = task;
// We need to distinguish the values forbidden by the user
// from the values forbidden by the algorithm.
// So we replace user's infinities by the maximum available double value.
normalize(step->matrix);
#ifdef DEBUG
qDebug() << step->matrix;
#endif // DEBUG
step->price = align(step->matrix);
root = step;
SStep *left, *right;
int nRow, nCol;
bool firstStep = true;
double check = INFINITY;
total = 0;
while (route.size() < nCities) {
step->alts = findCandidate(step->matrix,nRow,nCol);
while (hasSubCycles(nRow,nCol)) {
#ifdef DEBUG
qDebug() << "Forbidden: (" << nRow << ";" << nCol << ")";
#endif // DEBUG
step->matrix[nRow][nCol] = INFINITY;
step->price += align(step->matrix);
step->alts = findCandidate(step->matrix,nRow,nCol);
}
#ifdef DEBUG
qDebug() /*<< step->matrix*/ << "Selected: (" << nRow << ";" << nCol << ")";
qDebug() << "Alternate:" << step->alts;
qDebug() << "Step price:" << step->price << endl;
#endif // DEBUG
locker.relock();
if ((nRow == -1) || (nCol == -1) || canceled) {
if (canceled && cc)
cleanup();
return NULL;
}
locker.unlock();
// Route with (nRow,nCol) path
right = new SStep();
right->pNode = step;
right->matrix = step->matrix;
for (int k = 0; k < nCities; k++) {
if (k != nCol)
right->matrix[nRow][k] = INFINITY;
if (k != nRow)
right->matrix[k][nCol] = INFINITY;
}
right->price = step->price + align(right->matrix);
// Forbid the selected route to exclude its reuse in next steps.
right->matrix[nCol][nRow] = INFINITY;
right->matrix[nRow][nCol] = INFINITY;
// Route without (nRow,nCol) path
left = new SStep();
left->pNode = step;
left->matrix = step->matrix;
left->matrix[nRow][nCol] = INFINITY;
left->price = step->price + align(left->matrix);
step->candidate.nRow = nRow;
step->candidate.nCol = nCol;
step->plNode = left;
step->prNode = right;
// This matrix is not used anymore. Restoring infinities back.
denormalize(step->matrix);
if (right->price <= left->price) {
// Route with (nRow,nCol) path is cheaper
step->next = SStep::RightBranch;
step = right;
route[nRow] = nCol;
emit routePartFound(route.size());
if (firstStep) {
check = left->price;
firstStep = false;
}
} else {
// Route without (nRow,nCol) path is cheaper
step->next = SStep::LeftBranch;
step = left;
QCoreApplication::processEvents();
if (firstStep) {
check = right->price;
firstStep = false;
}
}
total++;
}
mayNotBeOptimal = (check < step->price);
return root;
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(vehicle)->getGlID())) {
int bla = 0;
}
#endif
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
assert(preb.size() == myChanger.size());
for (int i = 0; i < (int) myChanger.size(); ++i) {
((std::vector<MSVehicle::LaneQ>&) preb)[i].occupation = myChanger[i].dens + preb[i].nextOccupation;
}
vehicle->getLaneChangeModel().prepareStep();
std::pair<MSVehicle* const, SUMOReal> leader = getRealThisLeader(myCandi);
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (myCandi != myChanger.begin() && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> rLead = getRealLeader(myCandi - 1);
std::pair<MSVehicle* const, SUMOReal> rFollow = getRealFollower(myCandi - 1);
state1 = change2right(leader, rLead, rFollow, preb);
if ((state1 & LCA_URGENT) != 0 || (state1 & LCA_SPEEDGAIN) != 0) {
state1 |= LCA_RIGHT;
}
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_RIGHT);
#endif
(myCandi - 1)->hoppedVeh = vehicle;
(myCandi - 1)->lane->myTmpVehicles.push_front(vehicle);
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi - 1)->lane);
(myCandi - 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi - 1)->dens += (myCandi - 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if ((myCandi + 1) != myChanger.end() && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> lLead = getRealLeader(myCandi + 1);
std::pair<MSVehicle* const, SUMOReal> lFollow = getRealFollower(myCandi + 1);
state2 = change2left(leader, lLead, lFollow, preb);
if ((state2 & LCA_URGENT) != 0 || (state2 & LCA_SPEEDGAIN) != 0) {
state2 |= LCA_LEFT;
}
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
//vehicle->getLaneChangeModel().setOwnState(state2|state1);
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_LEFT);
#endif
(myCandi + 1)->hoppedVeh = veh(myCandi);
(myCandi + 1)->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi + 1)->lane);
(myCandi + 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi + 1)->dens += (myCandi + 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
}
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
vehicle->getLaneChangeModel().setOwnState(state1);
}
// check whether the vehicles should be swapped
if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
// get the direction ...
ChangerIt target;
if ((state1 & (LCA_URGENT)) != 0) {
// ... wants to go right
target = myCandi - 1;
}
if ((state2 & (LCA_URGENT)) != 0) {
// ... wants to go left
target = myCandi + 1;
}
MSVehicle* prohibitor = target->lead;
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) {
prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh;
}
}
if (prohibitor != 0
&&
((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0
&&
(prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
!=
(vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
)
) {
// check for position and speed
if (prohibitor->getVehicleType().getLengthWithGap() - vehicle->getVehicleType().getLengthWithGap() == 0) {
// ok, may be swapped
// remove vehicle to swap with
MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor);
if (i != target->lane->myTmpVehicles.end()) {
MSVehicle* bla = *i;
assert(bla == prohibitor);
target->lane->myTmpVehicles.erase(i);
// set this vehicle
target->hoppedVeh = vehicle;
target->lane->myTmpVehicles.push_front(vehicle);
myCandi->hoppedVeh = prohibitor;
myCandi->lane->myTmpVehicles.push_front(prohibitor);
// leave lane and detectors
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
prohibitor->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
// patch position and speed
SUMOReal p1 = vehicle->getPositionOnLane();
vehicle->myState.myPos = prohibitor->myState.myPos;
prohibitor->myState.myPos = p1;
p1 = vehicle->getSpeed();
vehicle->myState.mySpeed = prohibitor->myState.mySpeed;
prohibitor->myState.mySpeed = p1;
// enter lane and detectors
vehicle->enterLaneAtLaneChange(target->lane);
prohibitor->enterLaneAtLaneChange(myCandi->lane);
// mark lane change
vehicle->getLaneChangeModel().changed();
vehicle->myLastLaneChangeOffset = 0;
prohibitor->getLaneChangeModel().changed();
prohibitor->myLastLaneChangeOffset = 0;
(myCandi)->dens += prohibitor->getVehicleType().getLengthWithGap();
(target)->dens += vehicle->getVehicleType().getLengthWithGap();
return true;
}
}
}
}
// Candidate didn't change lane.
myCandi->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->myLastLaneChangeOffset += DELTA_T;
(myCandi)->dens += vehicle->getVehicleType().getLengthWithGap();
return false;
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
if (vehicle->isStopped()) {
// stopped vehicles obviously should not change lanes. Usually this is
// prevent by appropriate bestLane distances
return false;
}
const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
if (!isOpposite && leader.first == 0) {
// no reason to change unless there is a leader
// or we are changing back to the propper direction
// XXX also check whether the leader is so far away as to be irrelevant
return false;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = isOpposite ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ vehicle->getVehicleType().getLengthWithGap());
// all leader vehicles on the current laneChanger edge are already moved into MSLane::myTmpVehicles
const bool checkTmpVehicles = (&columnLeader.first->getLane()->getEdge() == &source->getEdge());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap,
checkTmpVehicles);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
if (leadLead.second > requiredSpace) {
foundSpaceAhead = true;
} else {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " not enough space after columnLeader=" << columnLeader.first->getID() << " required=" << requiredSpace << "\n";
}
#endif
seen += MAX2((SUMOReal)0, leadLead.second) + leadLead.first->getVehicleType().getLengthWithGap();
if (seen > OPPOSITE_OVERTAKING_MAX_LOOKAHEAD) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << columnLeader.first->getID() << ")\n";
}
#endif
return false;
}
// see if merging after leadLead is possible
egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
}
#endif
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// check for upcoming stops
if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake, true);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " changeOpposite opposite=" << opposite->getID()
<< " lead=" << Named::getIDSecure(leader.first)
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< "\n";
}
#endif
// check for dangerous oncoming leader
if (neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " oncoming=" << oncoming->getID()
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
} else {
timeToOvertake = -1;
// look forward as far as possible
spaceToOvertake = std::numeric_limits<SUMOReal>::max();
leader = source->getOppositeLeader(vehicle, OPPOSITE_OVERTAKING_ONCOMING_LOOKAHEAD, true);
// -1 will use getMaximumBrakeDist() as look-ahead distance
neighLead = opposite->getOppositeLeader(vehicle, -1, false);
}
// compute remaining space on the opposite side
// 1. the part that remains on the current lane
SUMOReal usableDist = isOpposite ? vehicle->getPositionOnLane() : source->getLength() - vehicle->getPositionOnLane();
if (usableDist < spaceToOvertake) {
// look forward along the next lanes
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
assert(bestLaneConts.size() >= 1);
std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
while (usableDist < spaceToOvertake && it != bestLaneConts.end()) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " usableDist=" << usableDist << " opposite=" << Named::getIDSecure((*it)->getOpposite()) << "\n";
}
#endif
if ((*it)->getOpposite() == 0) {
// opposite lane ends
break;
}
// do not overtake past a minor link or turn
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER || link->getDirection() != LINKDIR_STRAIGHT) {
break;
}
}
usableDist += (*it)->getLength();
++it;
}
}
if (!isOpposite && usableDist < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient space (seen=" << usableDist << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " usableDist=" << usableDist << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
}
#endif
// compute wish to change
std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
if (isOpposite) {
// compute the remaining distance that can be drive on the opposite side
// this value will put into LaneQ.length of the leftmost lane
// @note: length counts from the start of the current lane
// @note: see MSLCM_LC2013::_wantsChange @1092 (isOpposite()
MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
// position on the target lane
const SUMOReal forwardPos = source->getOppositePos(vehicle->getPositionOnLane());
// consider usableDist (due to minor links or end of opposite lanes)
laneQ.length = MIN2(laneQ.length, usableDist + forwardPos);
// consider upcoming stops
laneQ.length = MIN2(laneQ.length, vehicle->nextStopDist() + forwardPos);
// consider oncoming leaders
if (leader.first != 0) {
laneQ.length = MIN2(laneQ.length, leader.second / 2 + forwardPos);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " found oncoming leader=" << leader.first->getID() << " gap=" << leader.second << "\n";
}
#endif
leader.first = 0; // ignore leader after this
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " veh=" << vehicle->getID() << " remaining dist=" << laneQ.length - forwardPos << " forwardPos=" << forwardPos << " laneQ.length=" << laneQ.length << "\n";
}
#endif
}
std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
bool changingAllowed = (state & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed
// do not change to the opposite direction for cooperative reasons
&& (isOpposite || (state & LCA_COOPERATIVE) == 0)) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
/// XXX compute a better lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
if (!isOpposite) {
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction
<< " opposite=" << Named::getIDSecure(opposite) << " state=" << toString((LaneChangeAction)state) << "\n";
}
#endif
return false;
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gDebugSelectedVehicle == vehicle->getID()) {
int bla = 0;
}
#endif
if (vehicle->getLaneChangeModel().isChangingLanes()) {
return continueChange(vehicle, myCandi);
}
if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) {
registerUnchanged(vehicle);
return false;
}
std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi);
if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite()) {
if (changeOpposite(leader)) {
return true;
}
registerUnchanged(vehicle);
return false;
}
#ifndef NO_TRACI
if (vehicle->isRemoteControlled()) {
return false; // !!! temporary; just because it broke, here
}
#endif
vehicle->updateBestLanes(); // needed?
for (int i = 0; i < (int) myChanger.size(); ++i) {
vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
}
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (mayChange(-1)) {
state1 = checkChangeWithinEdge(-1, leader, preb);
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
vehicle->getLaneChangeModel().setOwnState(state1);
startChange(vehicle, myCandi, -1);
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
if ((myCandi - 1)->firstBlocked == 0) {
(myCandi - 1)->firstBlocked = vehicle;
}
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if (mayChange(1)) {
state2 = checkChangeWithinEdge(1, leader, preb);
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
vehicle->getLaneChangeModel().setOwnState(state2);
startChange(vehicle, myCandi, 1);
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
if ((myCandi + 1)->firstBlocked == 0) {
(myCandi + 1)->firstBlocked = vehicle;
}
}
}
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
// only emergency vehicles should change to the opposite side on a
// multi-lane road
if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
&& changeOpposite(leader)) {
return true;
} else {
registerUnchanged(vehicle);
return false;
}
}
/*!
* \brief Solves the given task.
* \param numCities Number of cities in the task.
* \param task The matrix of city-to-city travel costs.
* \return Pointer to the root of the solution tree.
*
* \todo TODO: Comment the algorithm.
*/
SStep* CTSPSolver::solve(int numCities, const TMatrix &task) {
if (numCities < 3) {
return NULL;
}
nCities = numCities;
SStep *step = new SStep(); // Initial node for the solution
step->matrix = task; // Copy the initial cost matrix
// Align the matrix and set the price of the first step to a lower bound for the
// entire algorithm.
step->price = align(step->matrix);
root = step;
SStep *left, *right;
int nRow, nCol;
bool firstStep = true;
double check = INFINITY;
total = 0;
while (route.size() < nCities) {
// For this step setup the alternative paths while also setting the nRow nCol for the next transition
step->alts = findCandidate(step->matrix, nRow, nCol);
// Continually align the matrix while the path has any subcycles. We also
// eliminate the currently considered path as we can determine this is not within
// the solution. For each align we need to update the currenly lower bound.
while (hasSubCycles(nRow,nCol)) {
step->matrix[nRow][nCol] = INFINITY; // Eliminate path from consideration
step->price += align(step->matrix); // Update lower bound
step->alts = findCandidate(step->matrix,nRow,nCol); // Get new best path
}
// A path could not be generated without subcycles; our algorithm has failed
if ((nRow == -1) || (nCol == -1)) {
return NULL;
}
// Create the inclusion transition. This will compute a new cost matrix with the
// the selected path as being within the soulution.
right = new SStep();
right->pNode = step;
right->matrix = step->matrix;
// Remove the to and from paths for the respective cities as we are selecting a path.
// This effectivly reduces the matrix from a NxN => (N-1)x(N-1)
for (int k = 0; k < nCities; k++) {
if (k != nCol) {
right->matrix[nRow][k] = INFINITY;
}
if (k != nRow) {
right->matrix[k][nCol] = INFINITY;
}
}
// By considering the path compute the new lower bound for the matrix
right->price = step->price + align(right->matrix);
// Remove the path to and symetrical path from from the cost matrix
right->matrix[nCol][nRow] = INFINITY;
right->matrix[nRow][nCol] = INFINITY;
// Create the left child for the current step and invalidate nRow and nCol
left = new SStep();
left->pNode = step;
left->matrix = step->matrix;
// Exclude this path from the solution
left->matrix[nRow][nCol] = INFINITY;
// Update the lower bound for this cost matrix with the path excluded
left->price = step->price + align(left->matrix);
// Book-keeping for the old GUI program
step->candidate.nRow = nRow;
step->candidate.nCol = nCol;
step->plNode = left;
step->prNode = right;
// If the right price is cheaper or equal then we will include the path into the solution
// and add this transition to the route. Then we will repeat the algorithm from the right child.
if (right->price <= left->price) {
step->next = SStep::RightBranch;
step = right;
route[nRow] = nCol;
if (firstStep) {
check = left->price;
firstStep = false;
}
// The exclusion path is cheaper and we will repeat the algorithm using the left child.
// IMPORTANT: We do not modify the route
} else {
step->next = SStep::LeftBranch;
step = left;
if (firstStep) {
check = right->price;
firstStep = false;
}
}
total++;
}
// On out first transition the step price is an estimated lower bound for the entire algorithm.
// If our final steps price is greater or equal then we cannot guarantee the solution is optimum.
mayNotBeOptimal = (check < step->price);
totalCost = step->price;
return root;
}
bool
MSLaneChanger::changeOpposite(std::pair<MSVehicle*, SUMOReal> leader) {
if (!myChangeToOpposite) {
return false;
}
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
MSLane* source = vehicle->getLane();
if (vehicle->isStopped()) {
// stopped vehicles obviously should not change lanes. Usually this is
// prevent by appropriate bestLane distances
return false;
}
const bool isOpposite = vehicle->getLaneChangeModel().isOpposite();
if (!isOpposite && leader.first == 0) {
// no reason to change unless there is a leader
// or we are changing back to the propper direction
// XXX also check whether the leader is so far away as to be irrelevant
return false;
}
if (!source->getEdge().canChangeToOpposite()) {
return false;
}
MSLane* opposite = source->getOpposite();
if (opposite == 0) {
return false;
}
// changing into the opposite direction is always to the left (XXX except for left-hand networkds)
int direction = vehicle->getLaneChangeModel().isOpposite() ? -1 : 1;
std::pair<MSVehicle*, SUMOReal> neighLead((MSVehicle*)0, -1);
// preliminary sanity checks for overtaking space
if (!isOpposite) {
assert(leader.first != 0);
// find a leader vehicle with sufficient space ahead for merging back
const SUMOReal overtakingSpeed = source->getVehicleMaxSpeed(vehicle); // just a guess
const SUMOReal mergeBrakeGap = vehicle->getCarFollowModel().brakeGap(overtakingSpeed);
const SUMOReal maxLookAhead = 150; // just a guess
std::pair<MSVehicle*, SUMOReal> columnLeader = leader;
SUMOReal egoGap = leader.second;
bool foundSpaceAhead = false;
SUMOReal seen = leader.second + leader.first->getVehicleType().getLengthWithGap();
std::vector<MSLane*> conts = vehicle->getBestLanesContinuation();
while (!foundSpaceAhead) {
const SUMOReal requiredSpaceAfterLeader = (columnLeader.first->getCarFollowModel().getSecureGap(
columnLeader.first->getSpeed(), overtakingSpeed, vehicle->getCarFollowModel().getMaxDecel())
+ vehicle->getVehicleType().getLengthWithGap());
std::pair<MSVehicle* const, SUMOReal> leadLead = columnLeader.first->getLane()->getLeader(
columnLeader.first, columnLeader.first->getPositionOnLane(), conts, requiredSpaceAfterLeader + mergeBrakeGap, true);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " leadLead=" << Named::getIDSecure(leadLead.first) << " gap=" << leadLead.second << "\n";
}
#endif
if (leadLead.first == 0) {
foundSpaceAhead = true;
} else {
const SUMOReal requiredSpace = (requiredSpaceAfterLeader
+ vehicle->getCarFollowModel().getSecureGap(overtakingSpeed, leadLead.first->getSpeed(), leadLead.first->getCarFollowModel().getMaxDecel()));
if (leadLead.second > requiredSpace) {
foundSpaceAhead = true;
} else {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " not enough space after columnLeader=" << leadLead.first->getID() << " gap=" << leadLead.second << " required=" << requiredSpace << "\n";
}
#endif
seen += leadLead.second + leadLead.first->getVehicleType().getLengthWithGap();
if (seen > maxLookAhead) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient free space after columnLeader (seen=" << seen << " columnLeader=" << leadLead.first->getID() << ")\n";
}
#endif
return false;
}
// see if merging after leadLead is possible
egoGap += columnLeader.first->getVehicleType().getLengthWithGap() + leadLead.second;
columnLeader = leadLead;
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " new columnLeader=" << columnLeader.first->getID() << "\n";
}
#endif
}
}
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " compute time/space to overtake for columnLeader=" << columnLeader.first->getID() << " gap=" << columnLeader.second << "\n";
}
#endif
SUMOReal timeToOvertake;
SUMOReal spaceToOvertake;
computeOvertakingTime(vehicle, columnLeader.first, egoGap, timeToOvertake, spaceToOvertake);
// check for upcoming stops
if (vehicle->nextStopDist() < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to upcoming stop (dist=" << vehicle->nextStopDist() << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
neighLead = opposite->getOppositeLeader(vehicle, timeToOvertake * opposite->getSpeedLimit() * 2 + spaceToOvertake);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " veh=" << vehicle->getID()
<< " changeOpposite opposite=" << opposite->getID()
<< " lead=" << Named::getIDSecure(leader.first)
<< " oncoming=" << Named::getIDSecure(neighLead.first)
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< "\n";
}
#endif
// check for dangerous oncoming leader
if (!vehicle->getLaneChangeModel().isOpposite() && neighLead.first != 0) {
const MSVehicle* oncoming = neighLead.first;
/// XXX what about overtaking multiple vehicles?
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME
<< " timeToOvertake=" << timeToOvertake
<< " spaceToOvertake=" << spaceToOvertake
<< " oncomingGap=" << neighLead.second
<< " leaderGap=" << leader.second
<< "\n";
}
#endif
if (neighLead.second - spaceToOvertake - timeToOvertake * oncoming->getSpeed() < 0) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to dangerous oncoming\n";
}
#endif
return false;
}
}
// check for sufficient space on the opposite side
seen = source->getLength() - vehicle->getPositionOnLane();
if (!vehicle->getLaneChangeModel().isOpposite() && seen < spaceToOvertake) {
const std::vector<MSLane*>& bestLaneConts = vehicle->getBestLanesContinuation();
assert(bestLaneConts.size() >= 1);
std::vector<MSLane*>::const_iterator it = bestLaneConts.begin() + 1;
while (seen < spaceToOvertake && it != bestLaneConts.end()) {
if ((*it)->getOpposite() == 0) {
break;
}
// do not overtake past a minor link
if (*(it - 1) != 0) {
MSLink* link = MSLinkContHelper::getConnectingLink(**(it - 1), **it);
if (link == 0 || !link->havePriority() || link->getState() == LINKSTATE_ZIPPER) {
break;
}
}
seen += (*it)->getLength();
}
if (seen < spaceToOvertake) {
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " cannot changeOpposite due to insufficient space (seen=" << seen << " spaceToOvertake=" << spaceToOvertake << ")\n";
}
#endif
return false;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << " seen=" << seen << " spaceToOvertake=" << spaceToOvertake << " timeToOvertake=" << timeToOvertake << "\n";
}
#endif
}
} else {
/// XXX compute sensible distance
leader = source->getOppositeLeader(vehicle, 200);
neighLead = opposite->getOppositeLeader(vehicle, -1);
}
// compute wish to change
std::vector<MSVehicle::LaneQ> preb = vehicle->getBestLanes();
if (isOpposite && leader.first != 0) {
MSVehicle::LaneQ& laneQ = preb[preb.size() - 1];
/// XXX compute sensible usable dist
laneQ.length -= MIN2(laneQ.length, opposite->getOppositePos(vehicle->getPositionOnLane()) + leader.second / 2);
leader.first = 0; // ignore leader
}
std::pair<MSVehicle* const, SUMOReal> neighFollow = opposite->getOppositeFollower(vehicle);
int state = checkChange(direction, opposite, leader, neighLead, neighFollow, preb);
bool changingAllowed = (state & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state & LCA_WANTS_LANECHANGE) != 0 && changingAllowed) {
vehicle->getLaneChangeModel().startLaneChangeManeuver(source, opposite, direction);
/// XXX use a dedicated transformation function
vehicle->myState.myPos = source->getOppositePos(vehicle->myState.myPos);
vehicle->myState.myBackPos = source->getOppositePos(vehicle->myState.myBackPos);
/// XXX compute a bette lateral position
opposite->forceVehicleInsertion(vehicle, vehicle->getPositionOnLane(), MSMoveReminder::NOTIFICATION_LANE_CHANGE, 0);
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return true;
}
#ifdef DEBUG_CHANGE_OPPOSITE
if (DEBUG_COND) {
std::cout << SIMTIME << " not changing to opposite veh=" << vehicle->getID() << " dir=" << direction << " opposite=" << Named::getIDSecure(opposite) << " state=" << state << "\n";
}
#endif
return false;
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gDebugSelectedVehicle == vehicle->getID()) {
int bla = 0;
}
#endif
if (vehicle->getLaneChangeModel().isChangingLanes()) {
return continueChange(vehicle, myCandi);
}
if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) {
registerUnchanged(vehicle);
return false;
}
std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi);
if (myChanger.size() == 1) {
if (changeOpposite(leader)) {
return true;
}
registerUnchanged(vehicle);
return false;
}
#ifndef NO_TRACI
if (vehicle->isRemoteControlled()) {
return false; // !!! temporary; just because it broke, here
}
#endif
vehicle->updateBestLanes(); // needed?
for (int i = 0; i < (int) myChanger.size(); ++i) {
vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
}
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (mayChange(-1)) {
state1 = checkChangeWithinEdge(-1, leader, preb);
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
vehicle->getLaneChangeModel().setOwnState(state1);
startChange(vehicle, myCandi, -1);
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
if ((myCandi - 1)->firstBlocked == 0) {
(myCandi - 1)->firstBlocked = vehicle;
}
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if (mayChange(1)) {
state2 = checkChangeWithinEdge(1, leader, preb);
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
vehicle->getLaneChangeModel().setOwnState(state2);
startChange(vehicle, myCandi, 1);
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
if ((myCandi + 1)->firstBlocked == 0) {
(myCandi + 1)->firstBlocked = vehicle;
}
}
}
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
// check whether the vehicles should be swapped
if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
// get the direction ...
ChangerIt target;
int direction = 0;
if ((state1 & (LCA_URGENT)) != 0) {
// ... wants to go right
target = myCandi - 1;
direction = -1;
}
if ((state2 & (LCA_URGENT)) != 0) {
// ... wants to go left
target = myCandi + 1;
direction = 1;
}
MSVehicle* prohibitor = target->lead;
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) {
prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh;
}
}
if (prohibitor != 0
&&
((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0
&&
(prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
!=
(vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
)
) {
// check for position and speed
if (prohibitor->getVehicleType().getLengthWithGap() == vehicle->getVehicleType().getLengthWithGap()) {
// ok, may be swapped
// remove vehicle to swap with
MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor);
if (i != target->lane->myTmpVehicles.end()) {
assert(*i == prohibitor);
target->lane->myTmpVehicles.erase(i);
startChange(vehicle, myCandi, direction);
startChange(prohibitor, target, -direction);
std::swap(vehicle->myState, prohibitor->myState);
myCandi->lead = prohibitor;
target->lead = vehicle;
return true;
}
}
}
}
if (!changeOpposite(leader)) {
registerUnchanged(vehicle);
return false;
} else {
return true;
}
}
