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