void SelectionSystem::CreateCurrentPath(Entity* unit, Entity* tile) { World* world = engine->GetContext()->GetWorld(); Pathfinder* pathfinder = engine->GetContext()->GetPathfinder(); int remainingAP = dynamic_cast<UnitComponent*>(unit->GetComponent(Component::UNIT))->ap; std::vector<Grid> path = pathfinder->FindPath(unit, tile); ClearCurrentPath(); // make new Entities with the selection sprite and add them to the engine int counter = 1; for (unsigned int i = 0; i < path.size(); i++) { Entity* step = new Entity(); if (counter <= remainingAP) { step->Add(new TextureComponent({ Graphics::SPRITE_PATH })); } else { step->Add(new TextureComponent({ Graphics::SPRITE_PATH_FAR })); } step->Add(new PositionComponent(path[i], 1)); engine->AddEntity(step); currentPath.push_back(step); counter++; } }
bool AISystem::MoveUnitTowardsEntity(Entity* selectedUnit, Entity* entity) { World* world = engine->GetContext()->GetWorld(); Pathfinder* pathfinder = engine->GetContext()->GetPathfinder(); vector<Grid> path = pathfinder->FindPath(selectedUnit, entity); if (path.size() > 0) { PositionComponent* upc = dynamic_cast<PositionComponent*>(selectedUnit->GetComponent(Component::POSITION)); Grid lastStep = *(path.end() - 1); SelectTarget(Graphics::Instance().ToPx(lastStep)); engine->GetContext()->SetGameState(Context::PL_MOVE); selectedUnit->Add(new AnimationComponent(path, upc->pos)); engine->UpdateEntity(selectedUnit); return true; } else { return false; } }
Entity* AISystem::FilterNearestUnit(Entity* selectedUnit, set<Entity*> units) { Pathfinder* pathfinder = engine->GetContext()->GetPathfinder(); Entity* nearest = nullptr; int pathSize; int shortestPathSize = INT_MAX; for (Entity* unit : units) { UnitComponent* uc = dynamic_cast<UnitComponent*>(unit->GetComponent(Component::UNIT)); vector<Grid> path = pathfinder->FindPurePath(selectedUnit, unit); pathSize = static_cast<int>(path.size()); if (pathSize > 0 && ( pathSize < shortestPathSize || (pathSize == shortestPathSize && uc->type == Entity::HQ) // if path sizes are equal, favor HQ ) ) { shortestPathSize = pathSize; nearest = unit; } } return nearest; }
int PersonajeModelo::mover(std::pair<int, int>& destino, float& velocidadAni) { Pathfinder pathF; int cambio = SIN_CAMBIO; double coste = 0; float costeF = 0; if (this->target == this->getPosition()) { return (this->quedarseQuieto(velocidadAni)); } if (esNecesarioCalcularNuevoPath()) { posMov = 0; caminoSize = 0; limpiarPath(); targetParcial.first = target.first; targetParcial.second = target.second; caminoSize = pathF.getPath(this->getPosition().first, this->getPosition().second, targetParcial.first, targetParcial.second, xPath, yPath); if (caminoSize == 0) { //Si no se tiene que mover, seteo el destino en los parciales this->orientar(target); target.first = targetParcial.first; target.second = targetParcial.second; } if (caminoSize < 0) { return (this->quedarseQuieto(velocidadAni)); } } if (posMov < caminoSize) { this->moverse(destino, velocidadAni); } else { return (this->quedarseQuieto(velocidadAni)); } cambio = ESTADO_MOVIMIENTO; estado = cambiarEstado(destino.first, destino.second, cambio); return estado; }
std::vector<int> GameState::getPath(int aSrc, int aTgt) const { if (grid_.offGrid(aSrc) || grid_.offGrid(aTgt)) return {}; Pathfinder pf; pf.setNeighbors([&] (int aIndex) {return getOpenNeighbors(aIndex);}); pf.setGoal(aTgt); return pf.getPathFrom(aSrc); }
std::vector<v3s16> get_path(ServerEnvironment* env, v3s16 source, v3s16 destination, unsigned int searchdistance, unsigned int max_jump, unsigned int max_drop, PathAlgorithm algo) { Pathfinder searchclass; return searchclass.getPath(env, source, destination, searchdistance, max_jump, max_drop, algo); }
/* pitstop callback */ static int pitcmd(int index, tCarElt* car, tSituation *s) { MyCar* myc = mycar[index-1]; Pathfinder* mpf = myc->getPathfinderPtr(); car->_pitFuel = MAX(MIN((car->_remainingLaps+1.0)*myc->fuelperlap - car->_fuel, car->_tank - car->_fuel), 0.0); myc->lastpitfuel = MAX(car->_pitFuel, 0.0); car->_pitRepair = car->_dammage; mpf->setPitStop(false, myc->getCurrentSegId()); myc->loadBehaviour(myc->START); myc->startmode = true; myc->trtime = 0.0; return ROB_PIT_IM; /* return immediately */ }
void GridNodeContainer::initNode(v3s16 ipos, PathGridnode *p_node) { INodeDefManager *ndef = m_pathf->m_env->getGameDef()->ndef(); PathGridnode &elem = *p_node; v3s16 realpos = m_pathf->getRealPos(ipos); MapNode current = m_pathf->m_env->getMap().getNodeNoEx(realpos); MapNode below = m_pathf->m_env->getMap().getNodeNoEx(realpos + v3s16(0, -1, 0)); if ((current.param0 == CONTENT_IGNORE) || (below.param0 == CONTENT_IGNORE)) { DEBUG_OUT("Pathfinder: " << PP(realpos) << " current or below is invalid element" << std::endl); if (current.param0 == CONTENT_IGNORE) { elem.type = 'i'; DEBUG_OUT(PP(ipos) << ": " << 'i' << std::endl); } return; } //don't add anything if it isn't an air node if (ndef->get(current).walkable || !ndef->get(below).walkable) { DEBUG_OUT("Pathfinder: " << PP(realpos) << " not on surface" << std::endl); if (ndef->get(current).walkable) { elem.type = 's'; DEBUG_OUT(PP(ipos) << ": " << 's' << std::endl); } else { elem.type = '-'; DEBUG_OUT(PP(ipos) << ": " << '-' << std::endl); } return; } elem.valid = true; elem.pos = realpos; elem.type = 'g'; DEBUG_OUT(PP(ipos) << ": " << 'a' << std::endl); if (m_pathf->m_prefetch) { elem.directions[DIR_XP] = m_pathf->calcCost(realpos, v3s16( 1, 0, 0)); elem.directions[DIR_XM] = m_pathf->calcCost(realpos, v3s16(-1, 0, 0)); elem.directions[DIR_ZP] = m_pathf->calcCost(realpos, v3s16( 0, 0, 1)); elem.directions[DIR_ZM] = m_pathf->calcCost(realpos, v3s16( 0, 0,-1)); } }
// Pitstop callback. static int pitcmd(int index, tCarElt* car, tSituation *s) { MyCar* myc = mycar[index-1]; Pathfinder* mpf = myc->getPathfinderPtr(); float fullracedist = (myTrackDesc->getTorcsTrack()->length*s->_totLaps); float remaininglaps = (fullracedist - car->_distRaced)/myTrackDesc->getTorcsTrack()->length; car->_pitFuel = MAX(MIN(myc->fuelperlap*(remaininglaps+myc->FUEL_SAFETY_MARGIN) - car->_fuel, car->_tank - car->_fuel), 0.0); myc->lastpitfuel = MAX(car->_pitFuel, 0.0); car->_pitRepair = car->_dammage; mpf->setPitStop(false, myc->getCurrentSegId()); myc->loadBehaviour(myc->START); myc->startmode = true; myc->trtime = 0.0; return ROB_PIT_IM; // Return immediately. }
PathfindingApp::PathfindingApp() { // Initialise SDL if (SDL_Init(SDL_INIT_VIDEO) < 0) { freopen("CON", "w", stdout); freopen("CON", "w", stderr); showSdlError("SDL_Init failed"); return; } freopen("CON", "w", stdout); freopen("CON", "w", stderr); // Load map map = new Map(MAP_NAME); // Calculate tile size, based on window height of 800 pixels tileSize = 800 / map->getHeight(); // Create a window and renderer window = SDL_CreateWindow("Pathfinding", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, map->getWidth() * tileSize, map->getHeight() * tileSize, SDL_WINDOW_SHOWN); renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC); // Load sprites spriteWall = IMG_LoadTexture(renderer, "Sprites\\brickGrey.png"); spriteMouse = IMG_LoadTexture(renderer, "Sprites\\mouse.png"); spriteExit = IMG_LoadTexture(renderer, "Sprites\\signExit.png"); // Check for errors in loading sprites if (!spriteWall || !spriteMouse || !spriteExit) { showSdlError("IMG_LoadTexture failed"); return; } // Find the path Pathfinder pathfinder; path = pathfinder.findPath(*map, map->getStartPoint(), map->getEndPoint()); }
std::vector<int> GameState::getReachableHexes(const Unit &unit) const { std::vector<int> reachable; // Flying units don't need a clear path. if (unit.canFly()) { for (int i = 0; i < grid_.size(); ++i) { if (isHexFlyable(unit, i)) { reachable.push_back(i); } } } else { Pathfinder pf; pf.setNeighbors([&] (int aIndex) {return getOpenNeighbors(aIndex);}); reachable = pf.getReachableNodes(unit.aHex, unit.type->moves); } return reachable; }
bool AISystem::MoveUnitTowardsEnemy(Entity* selectedUnit, Entity* enemy) { World* world = engine->GetContext()->GetWorld(); Pathfinder* pathfinder = engine->GetContext()->GetPathfinder(); vector<Grid> path = pathfinder->FindPurePath(selectedUnit, enemy); if (path.size() > 0) { UnitComponent* uc = dynamic_cast<UnitComponent*>(selectedUnit->GetComponent(Component::UNIT)); PositionComponent* upc = dynamic_cast<PositionComponent*>(selectedUnit->GetComponent(Component::POSITION)); Grid lastStep = *(path.end() - 1); SelectTarget(Graphics::Instance().ToPx(lastStep)); for (unsigned int i = 0; i <= uc->range_max; i++) { // last step is enemy unit so selectedUnit should not move there // unit should not move closer to enemy then is necessary to attack it path.pop_back(); if (path.size() == 1) { // path should not disappear entirely break; } } engine->GetContext()->SetGameState(Context::PL_MOVE); selectedUnit->Add(new AnimationComponent(path, upc->pos)); engine->UpdateEntity(selectedUnit); return true; } else { return false; } }
Entity* AISystem::FilterNearestEntity(Entity* selectedUnit, set<Entity*> entities) { Pathfinder* pathfinder = engine->GetContext()->GetPathfinder(); Entity* nearest = nullptr; int pathSize; int shortestPathSize = INT_MAX; for (Entity* entity : entities) { vector<Grid> path = pathfinder->FindPath(selectedUnit, entity); pathSize = static_cast<int>(path.size()); if (pathSize > 0 && pathSize < shortestPathSize) { shortestPathSize = pathSize; nearest = entity; } } return nearest; }
int main(int argc, char** argv) { srand(time(NULL)); cout << "Pathfinder V2 : THE DEMINER"<< endl << "---------------------------" <<endl; bool restart(false); Pathfinder pathfinder; cout << "Pathfinder cree"; outPut moteur(pathfinder.getMatrix()); moteur.init_outPut(); vector<int*> bombs = moteur.choosePoints(); vector<Node> resultList; vector<Node>* resultPointer; if(moteur.getStatus().running) resultList=pathfinder.find(bombs, moteur); if(resultList.empty() && moteur.getStatus().running) { cout << "Erreur" << endl << "Le probleme n'a pas de solution" << endl << "Annulation de l'operation" << endl; resultPointer = NULL; } else { moteur.drawResult(&resultList); cout << "La recherche de chemin est une reussite" << endl; resultPointer = &resultList; } while(moteur.getStatus().running) { moteur.setScene(); moteur.drawScene(); moteur.drawResult(resultPointer); moteur.display(); } return 0; }
void connectionManager::releaseIfNotUsed(long inputTimeCoordinate, long connectionPinHeight, Pathfinder& pathfinder, bool checkInPoints) { for(int boxType=0; boxType<2; boxType++) { std::set<size_t> toRelease; for(std::set<size_t>::iterator it = pool.toBeAvailable[boxType].begin(); it != pool.toBeAvailable[boxType].end(); it++) { /*compute connection pin coordinates based on heuristic and depth*/ int connNr = *it; pinpair connPins = getConnectionPinPair(connNr, inputTimeCoordinate, connectionPinHeight); printf("712: check conn %d @ coord %d\n", connNr, inputTimeCoordinate); bool isReleased = true; if(checkInPoints) { for(int i = 0; i < 2; i++) { Point* point = pathfinder.getOrCreatePoint(connPins.getPinDetail(i).coord[CIRCUITWIDTH], connPins.getPinDetail(i).coord[CIRCUITDEPTH], connPins.getPinDetail(i).coord[CIRCUITHEIGHT], false /*no boxes should exist in future*/); printf("712: %s\n", connPins.getPinDetail(i).coord.toString(',').c_str()); isReleased = isReleased && point->isFree(); } } if(isReleased) { toRelease.insert(connNr); } } for(std::set<size_t>::iterator it = toRelease.begin(); it != toRelease.end(); it++) { int connNr = *it; pool.releaseConnection(boxType, connNr); } } }
// Controls the car. static void drive(int index, tCarElt* car, tSituation *situation) { tdble angle; tdble brake; tdble b1; // Brake value in case we are to fast HERE and NOW. tdble b2; // Brake value for some brake point in front of us. tdble b3; // Brake value for control (avoid loosing control). tdble b4; // Brake value for avoiding high angle of attack. tdble b5; // Brake for the pit; tdble steer, targetAngle, shiftaccel; MyCar* myc = mycar[index-1]; Pathfinder* mpf = myc->getPathfinderPtr(); b1 = b2 = b3 = b4 = b5 = 0.0; shiftaccel = 0.0; // Update some values needed. myc->update(myTrackDesc, car, situation); // Decide how we want to drive, choose a behaviour. if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) { myc->loadBehaviour(myc->NORMAL); } else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) { myc->loadBehaviour(myc->CAREFUL); } else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) { myc->loadBehaviour(myc->SLOW); } // Update the other cars just once. if (currenttime != situation->currentTime) { currenttime = situation->currentTime; for (int i = 0; i < situation->_ncars; i++) ocar[i].update(); } // Startmode. if (myc->trtime < 5.0 && myc->bmode != myc->START) { myc->loadBehaviour(myc->START); myc->startmode = true; } if (myc->startmode && myc->trtime > 5.0) { myc->startmode = false; myc->loadBehaviour(myc->NORMAL); } // Compute path according to the situation. mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar); // Clear ctrl structure with zeros and set the current gear. memset(&car->ctrl, 0, sizeof(tCarCtrl)); car->_gearCmd = car->_gear; // Uncommenting the following line causes pitstop on every lap. //if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId()); // Compute fuel consumption. if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) { if (car->race.laps > 0) { myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel)); } myc->lastfuel = car->priv.fuel; myc->lastpitfuel = 0.0; myc->fuelchecked = true; } else if (myc->getCurrentSegId() > 5) { myc->fuelchecked = false; } // Decide if we need a pit stop. if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE || (car->priv.fuel < (myc->fuelperlap*(1.0+myc->FUEL_SAFETY_MARGIN)) && car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap))) { mpf->setPitStop(true, myc->getCurrentSegId()); } if (mpf->getPitStop()) { car->_raceCmd = RM_CMD_PIT_ASKED; } // Steer toward the next target point. targetAngle = atan2(myc->dynpath->getLoc(myc->destpathsegid)->y - car->_pos_Y, myc->dynpath->getLoc(myc->destpathsegid)->x - car->_pos_X); targetAngle -= car->_yaw; NORM_PI_PI(targetAngle); steer = targetAngle / car->_steerLock; // Steer P (proportional) controller. We add a steer correction proportional to the distance error // to the path. // Steer angle has usual meaning, therefore + is to left (CCW) and - to right (CW). // derror sign is + to right and - to left. if (!mpf->getPitStop()) { steer = steer + MIN(myc->STEER_P_CONTROLLER_MAX, myc->derror*myc->STEER_P_CONTROLLER_GAIN)*myc->getErrorSgn(); if (fabs(steer) > 1.0) { steer/=fabs(steer); } } else { // Check if we are almost in the pit to set brake to the max to avoid overrun. tdble dl, dw; RtDistToPit(car, myTrackDesc->getTorcsTrack(), &dl, &dw); if (dl < 1.0f) { b5 = 1.0f; } } // Try to control angular velocity with a D (differential) controller. double omega = myc->getSpeed()/myc->dynpath->getRadius(myc->currentpathsegid); steer += myc->STEER_D_CONTROLLER_GAIN*(omega - myc->getCarPtr()->_yaw_rate); // Brakes. tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION); tdble brakespeed, brakedist; tdble lookahead = 0.0; int i = myc->getCurrentSegId(); brake = 0.0; while (lookahead < brakecoeff * myc->getSpeedSqr()) { lookahead += myc->dynpath->getLength(i); brakespeed = myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(i); if (brakespeed > 0.0) { tdble gm, qb, qs; gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha(); qs = myc->dynpath->getSpeedsqr(i); brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw))); if (brakedist > lookahead - myc->getWheelTrack()) { qb = brakespeed*brakecoeff/brakedist; if (qb > b2) { b2 = qb; } } } i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg(); } if (myc->getSpeedSqr() > myc->dynpath->getSpeedsqr(myc->currentpathsegid)) { b1 = (myc->getSpeedSqr() - myc->dynpath->getSpeedsqr(myc->currentpathsegid)) / (myc->getSpeedSqr()); } // Try to avoid flying. if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) { b4 = 1.0; } // Check if we are on the way. if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) { if (myc->derror > myc->PATHERR) { vec2d *cd = myc->getDir(); vec2d *pd = myc->dynpath->getDir(myc->currentpathsegid); float z = cd->x*pd->y - cd->y*pd->x; // If the car points away from the path brake. if (z*myc->getErrorSgn() >= 0.0) { targetAngle = atan2(myc->dynpath->getDir(myc->currentpathsegid)->y, myc->dynpath->getDir(myc->currentpathsegid)->x); targetAngle -= car->_yaw; NORM_PI_PI(targetAngle); double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos()))); b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder; } } } // Anti wheel locking and brake code. if (b1 > b2) brake = b1; else brake = b2; if (brake < b3) brake = b3; if (brake < b4) { brake = MIN(1.0, b4); tdble abs_mean; abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed(); abs_mean /= 2.0; brake = brake * abs_mean; } else { brake = MIN(1.0, brake); tdble abs_min = 1.0; for (int i = 0; i < 4; i++) { tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed(); if (slip < abs_min) abs_min = slip; } brake = brake * abs_min; } // Reduce brake value to the approximate normal force available on the wheels. float weight = myc->mass*G; float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED; float force = weight + myc->ca*myc->getSpeedSqr(); brake = brake*MIN(1.0, force/maxForce); if (b5 > 0.0f) { brake = b5; } // Gear changing. if (myc->tr_mode == 0) { if (car->_gear <= 0) { car->_gearCmd = 1; } else { float gr_up = car->_gearRatio[car->_gear + car->_gearOffset]; float omega = car->_enginerpmRedLine/gr_up; float wr = car->_wheelRadius(2); if (omega*wr*myc->SHIFT < car->_speed_x) { car->_gearCmd++; } else { float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1]; omega = car->_enginerpmRedLine/gr_down; if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) { car->_gearCmd--; } } } } // Acceleration / brake execution. tdble cerror, cerrorh; cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y); if (cerrorh > myc->TURNSPEED) { cerror = fabs(car->_speed_x)/cerrorh; } else { cerror = 1.0; } if (myc->tr_mode == 0) { if (brake > 0.0) { myc->accel = 0.0; car->_accelCmd = myc->accel; car->_brakeCmd = brake*cerror; } else { if (myc->getSpeedSqr() < myc->dynpath->getSpeedsqr(myc->getCurrentSegId())) { if (myc->accel < myc->ACCELLIMIT) { myc->accel += myc->ACCELINC; } car->_accelCmd = myc->accel/cerror; } else { if (myc->accel > 0.0) { myc->accel -= myc->ACCELINC; } // TODO: shiftaccel always 0 at the moment... car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror); } tdble slipspeed = myc->querySlipSpeed(car); if (slipspeed > myc->TCL_SLIP) { car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE); } } } // Check if we are stuck, try to get unstuck. tdble bx = myc->getDir()->x, by = myc->getDir()->y; tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y; tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by)); if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) { myc->turnaround += situation->deltaTime; } else myc->turnaround = 0.0; if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) { if (myc->tr_mode == 0) { myc->tr_mode = 1; } if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) { car->_accelCmd = 0.0; if (myc->tr_mode == 1) { car->_gearCmd--; } car->_brakeCmd = 1.0; } else { myc->tr_mode = 2; if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), myc->dynpath->getLoc(myc->getCurrentSegId())) > myc->TURNTOL)) { angle = queryAngleToTrack(car); car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0; car->_brakeCmd = 0.0; if (myc->accel < 1.0) { myc->accel += myc->ACCELINC; } car->_accelCmd = myc->accel; tdble slipspeed = myc->querySlipSpeed(car); if (slipspeed < -myc->TCL_SLIP) { car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE); } } else { if (myc->getSpeed() < 1.0) { myc->turnaround = 0; myc->tr_mode = 0; myc->loadBehaviour(myc->START); myc->startmode = true; myc->trtime = 0.0; } car->_brakeCmd = 1.0; car->_steerCmd = 0.0; car->_accelCmd = 0.0; } } } if (myc->tr_mode == 0) car->_steerCmd = steer; car->_clutchCmd = getClutch(myc, car); }
Json::Value PathRequest::doUpdate (RippleLineCache::ref cache, bool fast) { m_journal.debug << iIdentifier << " update " << (fast ? "fast" : "normal"); ScopedLockType sl (mLock); if (!isValid (cache)) return jvStatus; jvStatus = Json::objectValue; auto sourceCurrencies = sciSourceCurrencies; if (sourceCurrencies.empty ()) { auto usCurrencies = usAccountSourceCurrencies (raSrcAccount, cache, true); bool sameAccount = raSrcAccount == raDstAccount; for (auto const& c: usCurrencies) { if (!sameAccount || (c != saDstAmount.getCurrency ())) { if (c.isZero ()) sourceCurrencies.insert (std::make_pair (c, xrpAccount())); else sourceCurrencies.insert (std::make_pair (c, raSrcAccount.getAccountID ())); } } } jvStatus["source_account"] = raSrcAccount.humanAccountID (); jvStatus["destination_account"] = raDstAccount.humanAccountID (); jvStatus["destination_amount"] = saDstAmount.getJson (0); if (!jvId.isNull ()) jvStatus["id"] = jvId; Json::Value jvArray = Json::arrayValue; int iLevel = iLastLevel; bool loaded = getApp().getFeeTrack().isLoadedLocal(); if (iLevel == 0) { // first pass if (loaded || fast) iLevel = getConfig().PATH_SEARCH_FAST; else iLevel = getConfig().PATH_SEARCH; } else if ((iLevel == getConfig().PATH_SEARCH_FAST) && !fast) { // leaving fast pathfinding iLevel = getConfig().PATH_SEARCH; if (loaded && (iLevel > getConfig().PATH_SEARCH_FAST)) --iLevel; } else if (bLastSuccess) { // decrement, if possible if (iLevel > getConfig().PATH_SEARCH || (loaded && (iLevel > getConfig().PATH_SEARCH_FAST))) --iLevel; } else { // adjust as needed if (!loaded && (iLevel < getConfig().PATH_SEARCH_MAX)) ++iLevel; if (loaded && (iLevel > getConfig().PATH_SEARCH_FAST)) --iLevel; } m_journal.debug << iIdentifier << " processing at level " << iLevel; bool found = false; for (auto const& currIssuer: sourceCurrencies) { { STAmount test ({currIssuer.first, currIssuer.second}, 1); if (m_journal.debug) { m_journal.debug << iIdentifier << " Trying to find paths: " << test.getFullText (); } } bool valid; STPathSet& spsPaths = mContext[currIssuer]; Pathfinder pf (cache, raSrcAccount, raDstAccount, currIssuer.first, currIssuer.second, saDstAmount, valid); CondLog (!valid, lsDEBUG, PathRequest) << iIdentifier << " PF request not valid"; STPath extraPath; if (valid && pf.findPaths (iLevel, 4, spsPaths, extraPath)) { LedgerEntrySet lesSandbox (cache->getLedger (), tapNONE); PathState::List pathStateList; STAmount saMaxAmountAct; STAmount saDstAmountAct; auto& account = currIssuer.second.isNonZero () ? Account(currIssuer.second) : isXRP (currIssuer.first) ? xrpAccount() : raSrcAccount.getAccountID (); STAmount saMaxAmount ({currIssuer.first, account}, 1); saMaxAmount.negate (); m_journal.debug << iIdentifier << " Paths found, calling rippleCalc"; TER resultCode = path::rippleCalculate ( lesSandbox, saMaxAmountAct, saDstAmountAct, pathStateList, saMaxAmount, saDstAmount, raDstAccount.getAccountID (), raSrcAccount.getAccountID (), spsPaths, false, false, false, true); if ((extraPath.size() > 0) && ((resultCode == terNO_LINE) || (resultCode == tecPATH_PARTIAL))) { m_journal.debug << iIdentifier << " Trying with an extra path element"; spsPaths.addPath(extraPath); pathStateList.clear (); resultCode = path::rippleCalculate ( lesSandbox, saMaxAmountAct, saDstAmountAct, pathStateList, saMaxAmount, saDstAmount, raDstAccount.getAccountID (), raSrcAccount.getAccountID (), spsPaths, false, false, false, true); m_journal.debug << iIdentifier << " Extra path element gives " << transHuman (resultCode); } if (resultCode == tesSUCCESS) { Json::Value jvEntry (Json::objectValue); jvEntry["source_amount"] = saMaxAmountAct.getJson (0); jvEntry["paths_computed"] = spsPaths.getJson (0); found = true; jvArray.append (jvEntry); } else { m_journal.debug << iIdentifier << " rippleCalc returns " << transHuman (resultCode); } } else { m_journal.debug << iIdentifier << " No paths found"; } } iLastLevel = iLevel; bLastSuccess = found; if (fast && ptQuickReply.is_not_a_date_time()) { ptQuickReply = boost::posix_time::microsec_clock::universal_time(); mOwner.reportFast ((ptQuickReply-ptCreated).total_milliseconds()); } else if (!fast && ptFullReply.is_not_a_date_time()) { ptFullReply = boost::posix_time::microsec_clock::universal_time(); mOwner.reportFull ((ptFullReply-ptCreated).total_milliseconds()); } jvStatus["alternatives"] = jvArray; return jvStatus; }
/* controls the car */ static void drive(int index, tCarElt* car, tSituation *situation) { tdble angle; tdble brake; tdble b1; /* brake value in case we are to fast HERE and NOW */ tdble b2; /* brake value for some brake point in front of us */ tdble b3; /* brake value for control (avoid loosing control) */ tdble b4; /* brake value for avoiding high angle of attack */ tdble steer, targetAngle, shiftaccel; MyCar* myc = mycar[index-1]; Pathfinder* mpf = myc->getPathfinderPtr(); b1 = b2 = b3 = b4 = 0.0; shiftaccel = 0.0; /* update some values needed */ myc->update(myTrackDesc, car, situation); /* decide how we want to drive */ if ( car->_dammage < myc->undamaged/3 && myc->bmode != myc->NORMAL) { myc->loadBehaviour(myc->NORMAL); } else if (car->_dammage > myc->undamaged/3 && car->_dammage < (myc->undamaged*2)/3 && myc->bmode != myc->CAREFUL) { myc->loadBehaviour(myc->CAREFUL); } else if (car->_dammage > (myc->undamaged*2)/3 && myc->bmode != myc->SLOW) { myc->loadBehaviour(myc->SLOW); } /* update the other cars just once */ if (currenttime != situation->currentTime) { currenttime = situation->currentTime; for (int i = 0; i < situation->_ncars; i++) ocar[i].update(); } /* startmode */ if (myc->trtime < 5.0 && myc->bmode != myc->START) { myc->loadBehaviour(myc->START); myc->startmode = true; } if (myc->startmode && myc->trtime > 5.0) { myc->startmode = false; myc->loadBehaviour(myc->NORMAL); } /* compute path according to the situation */ mpf->plan(myc->getCurrentSegId(), car, situation, myc, ocar); /* clear ctrl structure with zeros and set the current gear */ memset(&car->ctrl, 0, sizeof(tCarCtrl)); car->_gearCmd = car->_gear; /* uncommenting the following line causes pitstop on every lap */ //if (!mpf->getPitStop()) mpf->setPitStop(true, myc->getCurrentSegId()); /* compute fuel consumption */ if (myc->getCurrentSegId() >= 0 && myc->getCurrentSegId() < 5 && !myc->fuelchecked) { if (car->race.laps > 0) { myc->fuelperlap = MAX(myc->fuelperlap, (myc->lastfuel+myc->lastpitfuel-car->priv.fuel)); } myc->lastfuel = car->priv.fuel; myc->lastpitfuel = 0.0; myc->fuelchecked = true; } else if (myc->getCurrentSegId() > 5) { myc->fuelchecked = false; } /* decide if we need a pit stop */ if (!mpf->getPitStop() && (car->_remainingLaps-car->_lapsBehindLeader) > 0 && (car->_dammage > myc->MAXDAMMAGE || (car->priv.fuel < 1.5*myc->fuelperlap && car->priv.fuel < (car->_remainingLaps-car->_lapsBehindLeader)*myc->fuelperlap))) { mpf->setPitStop(true, myc->getCurrentSegId()); } if (mpf->getPitStop()) { car->_raceCmd = RM_CMD_PIT_ASKED; } /* steer to next target point */ targetAngle = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X); targetAngle -= car->_yaw; NORM_PI_PI(targetAngle); steer = targetAngle / car->_steerLock; /* brakes */ tdble brakecoeff = 1.0/(2.0*g*myc->currentseg->getKfriction()*myc->CFRICTION); tdble brakespeed, brakedist; tdble lookahead = 0.0; int i = myc->getCurrentSegId(); brake = 0.0; while (lookahead < brakecoeff * myc->getSpeedSqr()) { lookahead += mpf->getPathSeg(i)->getLength(); brakespeed = myc->getSpeedSqr() - mpf->getPathSeg(i)->getSpeedsqr(); if (brakespeed > 0.0) { tdble gm, qb, qs; gm = myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKfriction()*myc->CFRICTION*myTrackDesc->getSegmentPtr(myc->getCurrentSegId())->getKalpha(); qs = mpf->getPathSeg(i)->getSpeedsqr(); brakedist = brakespeed*(myc->mass/(2.0*gm*g*myc->mass + qs*(gm*myc->ca + myc->cw))); if (brakedist > lookahead - myc->getWheelTrack()) { qb = brakespeed*brakecoeff/brakedist; if (qb > b2) { b2 = qb; } } } i = (i + 1 + mpf->getnPathSeg()) % mpf->getnPathSeg(); } if (myc->getSpeedSqr() > myc->currentpathseg->getSpeedsqr()) { b1 = (myc->getSpeedSqr() - myc->currentpathseg->getSpeedsqr()) / (myc->getSpeedSqr()); } /* try to avoid flying */ if (myc->getDeltaPitch() > myc->MAXALLOWEDPITCH && myc->getSpeed() > myc->FLYSPEED) { b4 = 1.0; } if (!mpf->getPitStop()) { steer = steer + MIN(0.1, myc->derror*0.02)*myc->getErrorSgn(); if (fabs(steer) > 1.0) steer/=fabs(steer); } /* check if we are on the way */ if (myc->getSpeed() > myc->TURNSPEED && myc->tr_mode == 0) { if (myc->derror > myc->PATHERR) { v3d r; myc->getDir()->crossProduct(myc->currentpathseg->getDir(), &r); if (r.z*myc->getErrorSgn() >= 0.0) { targetAngle = atan2(myc->currentpathseg->getDir()->y, myc->currentpathseg->getDir()->x); targetAngle -= car->_yaw; NORM_PI_PI(targetAngle); double toborder = MAX(1.0, myc->currentseg->getWidth()/2.0 - fabs(myTrackDesc->distToMiddle(myc->getCurrentSegId(), myc->getCurrentPos()))); b3 = (myc->getSpeed()/myc->STABLESPEED)*(myc->derror-myc->PATHERR)/toborder; } } } /* try to control angular velocity */ double omega = myc->getSpeed()/myc->currentpathseg->getRadius(); steer += 0.1*(omega - myc->getCarPtr()->_yaw_rate); /* anti blocking and brake code */ if (b1 > b2) brake = b1; else brake = b2; if (brake < b3) brake = b3; if (brake < b4) { brake = MIN(1.0, b4); tdble abs_mean; abs_mean = (car->_wheelSpinVel(REAR_LFT) + car->_wheelSpinVel(REAR_RGT))*car->_wheelRadius(REAR_LFT)/myc->getSpeed(); abs_mean /= 2.0; brake = brake * abs_mean; } else { brake = MIN(1.0, brake); tdble abs_min = 1.0; for (int i = 0; i < 4; i++) { tdble slip = car->_wheelSpinVel(i) * car->_wheelRadius(i) / myc->getSpeed(); if (slip < abs_min) abs_min = slip; } brake = brake * abs_min; } float weight = myc->mass*G; float maxForce = weight + myc->ca*myc->MAX_SPEED*myc->MAX_SPEED; float force = weight + myc->ca*myc->getSpeedSqr(); brake = brake*MIN(1.0, force/maxForce); // Gear changing. if (myc->tr_mode == 0) { if (car->_gear <= 0) { car->_gearCmd = 1; } else { float gr_up = car->_gearRatio[car->_gear + car->_gearOffset]; float omega = car->_enginerpmRedLine/gr_up; float wr = car->_wheelRadius(2); if (omega*wr*myc->SHIFT < car->_speed_x) { car->_gearCmd++; } else { float gr_down = car->_gearRatio[car->_gear + car->_gearOffset - 1]; omega = car->_enginerpmRedLine/gr_down; if (car->_gear > 1 && omega*wr*myc->SHIFT > car->_speed_x + myc->SHIFT_MARGIN) { car->_gearCmd--; } } } } tdble cerror, cerrorh; cerrorh = sqrt(car->_speed_x*car->_speed_x + car->_speed_y*car->_speed_y); if (cerrorh > myc->TURNSPEED) cerror = fabs(car->_speed_x)/cerrorh; else cerror = 1.0; /* acceleration / brake execution */ if (myc->tr_mode == 0) { if (brake > 0.0) { myc->accel = 0.0; car->_accelCmd = myc->accel; car->_brakeCmd = brake*cerror; } else { if (myc->getSpeedSqr() < mpf->getPathSeg(myc->getCurrentSegId())->getSpeedsqr()) { if (myc->accel < myc->ACCELLIMIT) { myc->accel += myc->ACCELINC; } car->_accelCmd = myc->accel/cerror; } else { if (myc->accel > 0.0) { myc->accel -= myc->ACCELINC; } car->_accelCmd = myc->accel = MIN(myc->accel/cerror, shiftaccel/cerror); } tdble slipspeed = myc->querySlipSpeed(car); if (slipspeed > myc->TCL_SLIP) { car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (slipspeed - myc->TCL_SLIP)/myc->TCL_RANGE); } } } /* check if we are stuck, try to get unstuck */ tdble bx = myc->getDir()->x, by = myc->getDir()->y; tdble cx = myc->currentseg->getMiddle()->x - car->_pos_X, cy = myc->currentseg->getMiddle()->y - car->_pos_Y; tdble parallel = (cx*bx + cy*by) / (sqrt(cx*cx + cy*cy)*sqrt(bx*bx + by*by)); if ((myc->getSpeed() < myc->TURNSPEED) && (parallel < cos(90.0*PI/180.0)) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) { myc->turnaround += situation->deltaTime; } else myc->turnaround = 0.0; if ((myc->turnaround >= waitToTurn) || (myc->tr_mode >= 1)) { if (myc->tr_mode == 0) { myc->tr_mode = 1; } if ((car->_gearCmd > -1) && (myc->tr_mode < 2)) { car->_accelCmd = 0.0; if (myc->tr_mode == 1) { car->_gearCmd--; } car->_brakeCmd = 1.0; } else { myc->tr_mode = 2; if (parallel < cos(90.0*PI/180.0) && (mpf->dist2D(myc->getCurrentPos(), mpf->getPathSeg(myc->getCurrentSegId())->getLoc()) > myc->TURNTOL)) { angle = queryAngleToTrack(car); car->_steerCmd = ( -angle > 0.0) ? 1.0 : -1.0; car->_brakeCmd = 0.0; if (myc->accel < 1.0) { myc->accel += myc->ACCELINC; } car->_accelCmd = myc->accel; tdble slipspeed = myc->querySlipSpeed(car); if (slipspeed < -myc->TCL_SLIP) { car->_accelCmd = car->_accelCmd - MIN(car->_accelCmd, (myc->TCL_SLIP - slipspeed)/myc->TCL_RANGE); } } else { if (myc->getSpeed() < 1.0) { myc->turnaround = 0; myc->tr_mode = 0; myc->loadBehaviour(myc->START); myc->startmode = true; myc->trtime = 0.0; } car->_brakeCmd = 1.0; car->_steerCmd = 0.0; car->_accelCmd = 0.0; } } } if (myc->tr_mode == 0) car->_steerCmd = steer; timeSinceLastUpdate[index - 1] += situation->deltaTime; //Only update once per second if(timeSinceLastUpdate[index - 1] >= 0.1) { /* steer to next target point */ float targetAngleOut = atan2(myc->destpathseg->getLoc()->y - car->_pos_Y, myc->destpathseg->getLoc()->x - car->_pos_X); targetAngleOut -= car->_yaw; NORM_PI_PI(targetAngleOut); timeSinceLastUpdate[index - 1] = 0.0; sensors[index - 1]->sensors_update(); //Write training data outputFiles[index - 1]<<"DATA"<<std::endl; //INPUT outputFiles[index - 1]<<"speed "<<myc->getSpeed()<<std::endl; outputFiles[index - 1]<<"angle "<<myc->getDeltaPitch()<<std::endl; outputFiles[index - 1]<<"targetAngle "<<targetAngleOut<<std::endl; //Distance sensors outputFiles[index - 1]<<"distR "<<sensors[index - 1]->getSensorOut(0)<<std::endl; outputFiles[index - 1]<<"distFR "<<sensors[index - 1]->getSensorOut(1)<<std::endl; outputFiles[index - 1]<<"distFFR "<<sensors[index - 1]->getSensorOut(2)<<std::endl; outputFiles[index - 1]<<"distF "<<sensors[index - 1]->getSensorOut(3)<<std::endl; outputFiles[index - 1]<<"distFFL "<<sensors[index - 1]->getSensorOut(4)<<std::endl; outputFiles[index - 1]<<"distFL "<<sensors[index - 1]->getSensorOut(5)<<std::endl; outputFiles[index - 1]<<"distL "<<sensors[index - 1]->getSensorOut(6)<<std::endl; //OUTPUT outputFiles[index - 1]<<"steer "<<car->ctrl.steer<<std::endl; outputFiles[index - 1]<<"accel "<<car->ctrl.accelCmd<<std::endl; outputFiles[index - 1]<<"brake "<<car->ctrl.brakeCmd<<std::endl; outputFiles[index - 1]<<"gear "<<car->ctrl.gear<<std::endl; outputFiles[index - 1]<<"clutch "<<car->ctrl.clutchCmd<<std::endl; //Write training data to console printf_s("-----------------------------\n"); printf_s("Speed: %f\n", myc->getSpeed()); printf_s("Steering: %f\n", car->ctrl.steer); printf_s("Acceleration: %f\n", car->ctrl.accelCmd); printf_s("Brake: %f\n", car->ctrl.brakeCmd); printf_s("Gear: %f\n", car->ctrl.gear); printf_s("Clutch: %f\n", car->ctrl.clutchCmd); printf_s("Angle: %f\n", myc->getDeltaPitch()); printf_s("Target Angle: %f\n\n", targetAngleOut); printf_s("distR %f\n", sensors[index - 1]->getSensorOut(0)); printf_s("distFR %f\n", sensors[index - 1]->getSensorOut(1)); printf_s("distFFR %f\n", sensors[index - 1]->getSensorOut(2)); printf_s("distF %f\n", sensors[index - 1]->getSensorOut(3)); printf_s("distFFL %f\n", sensors[index - 1]->getSensorOut(4)); printf_s("distFL %f\n", sensors[index - 1]->getSensorOut(5)); printf_s("distL %f\n", sensors[index - 1]->getSensorOut(6)); } }
static Json::Value signPayment( Json::Value const& params, Json::Value& tx_json, RippleAddress const& raSrcAddressID, Ledger::pointer lSnapshot, int role) { RippleAddress dstAccountID; if (!tx_json.isMember ("Amount")) return RPC::missing_field_error ("tx_json.Amount"); STAmount amount; if (!amount.bSetJson (tx_json ["Amount"])) return RPC::invalid_field_error ("tx_json.Amount"); if (!tx_json.isMember ("Destination")) return RPC::missing_field_error ("tx_json.Destination"); if (!dstAccountID.setAccountID (tx_json["Destination"].asString ())) return RPC::invalid_field_error ("tx_json.Destination"); if (tx_json.isMember ("Paths") && params.isMember ("build_path")) return RPC::make_error (rpcINVALID_PARAMS, "Cannot specify both 'tx_json.Paths' and 'tx_json.build_path'"); if (!tx_json.isMember ("Paths") && tx_json.isMember ("Amount") && params.isMember ("build_path")) { // Need a ripple path. STPathSet spsPaths; uint160 uSrcCurrencyID; uint160 uSrcIssuerID; STAmount saSendMax; if (tx_json.isMember ("SendMax")) { if (!saSendMax.bSetJson (tx_json ["SendMax"])) return RPC::invalid_field_error ("tx_json.SendMax"); } else { // If no SendMax, default to Amount with sender as issuer. saSendMax = amount; saSendMax.setIssuer (raSrcAddressID.getAccountID ()); } if (saSendMax.isNative () && amount.isNative ()) return RPC::make_error (rpcINVALID_PARAMS, "Cannot build STR to STR paths."); { LegacyPathFind lpf (role == Config::ADMIN); if (!lpf.isOk ()) return rpcError (rpcTOO_BUSY); bool bValid; auto cache = boost::make_shared<RippleLineCache> (lSnapshot); Pathfinder pf (cache, raSrcAddressID, dstAccountID, saSendMax.getCurrency (), saSendMax.getIssuer (), amount, bValid); STPath extraPath; if (!bValid || !pf.findPaths (getConfig ().PATH_SEARCH_OLD, 4, spsPaths, extraPath)) { WriteLog (lsDEBUG, RPCHandler) << "transactionSign: build_path: No paths found."; return rpcError (rpcNO_PATH); } WriteLog (lsDEBUG, RPCHandler) << "transactionSign: build_path: " << spsPaths.getJson (0); if (!spsPaths.isEmpty ()) tx_json["Paths"] = spsPaths.getJson (0); } } return Json::Value(); }