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;
}
Example #4
0
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;
}
Example #5
0
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);
}
Example #6
0
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);
}
Example #7
0
/* 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 */
}
Example #8
0
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));
    }
}
Example #9
0
// 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());
}
Example #11
0
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;
}
Example #14
0
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;
}
Example #15
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);
		}
	}
}
Example #16
0
// 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);
}
Example #17
0
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;
}
Example #18
0
/* 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();
}