diff --git a/src/drivers/shadow/CMakeLists.txt b/src/drivers/shadow/CMakeLists.txt index f188fb7fa..72314f084 100644 --- a/src/drivers/shadow/CMakeLists.txt +++ b/src/drivers/shadow/CMakeLists.txt @@ -11,6 +11,8 @@ SET(ROBOT_SOURCES src/Array.h src/AveragedData.h src/Avoidance.cpp src/Avoidance.h + src/CarBounds2d.cpp + src/CarBounds2d.h src/CarModel.cpp src/CarModel.h src/ClothoidPath.cpp @@ -58,6 +60,8 @@ SET(ROBOT_SOURCES src/Array.h src/Span.h src/Strategy.cpp src/Strategy.h + src/Stuck.cpp + src/Stuck.h src/TeamInfo.cpp src/TeamInfo.h src/Utils.cpp diff --git a/src/drivers/shadow/src/CarBounds2d.cpp b/src/drivers/shadow/src/CarBounds2d.cpp new file mode 100644 index 000000000..db6da727c --- /dev/null +++ b/src/drivers/shadow/src/CarBounds2d.cpp @@ -0,0 +1,242 @@ + +#include "CarBounds2d.h" +#include "Utils.h" + +using namespace std; + +static const int s_next_corner[] = {FRNT_LFT, REAR_LFT, REAR_RGT, FRNT_RGT}; + +CarBounds2d::CarBounds2d( const tCarElt* car ) +{ +/* + xAxis = Vec2d(car->pub.DynGC.pos.az); + yAxis = xAxis.GetNormal(); + + Vec2d middle(car->pub.DynGC.pos.x, car->pub.DynGC.pos.y); + + pts[FRNT_LFT] = middle + xAxis * car->pub.corner[FRNT_LFT].x + yAxis * car->pub.corner[FRNT_LFT].y; + pts[FRNT_RGT] = middle + xAxis * car->pub.corner[FRNT_RGT].x + yAxis * car->pub.corner[FRNT_RGT].y; + pts[REAR_LFT] = middle + xAxis * car->pub.corner[REAR_LFT].x + yAxis * car->pub.corner[REAR_LFT].y; + pts[REAR_RGT] = middle + xAxis * car->pub.corner[REAR_RGT].x + yAxis * car->pub.corner[REAR_RGT].y; +*/ + pts[FRNT_LFT] = Vec2d(car->pub.corner[FRNT_LFT].ax, car->pub.corner[FRNT_LFT].ay); + pts[FRNT_RGT] = Vec2d(car->pub.corner[FRNT_RGT].ax, car->pub.corner[FRNT_RGT].ay); + pts[REAR_LFT] = Vec2d(car->pub.corner[REAR_LFT].ax, car->pub.corner[REAR_LFT].ay); + pts[REAR_RGT] = Vec2d(car->pub.corner[REAR_RGT].ax, car->pub.corner[REAR_RGT].ay); + + xAxis = Vec2d(pts[FRNT_LFT] - pts[REAR_LFT]).GetUnit(); + yAxis = Vec2d(pts[FRNT_LFT] - pts[FRNT_RGT]).GetUnit(); +} + +double CarBounds2d::distToSide( int side, double maxDist, const CarBounds2d& other ) const +{ + vector pts; + pts.push_back( other.pts[FRNT_RGT] ); + pts.push_back( other.pts[FRNT_LFT] ); + pts.push_back( other.pts[REAR_LFT] ); + pts.push_back( other.pts[REAR_RGT] ); + pts.push_back( other.pts[FRNT_RGT] ); + + return distToSide(side, maxDist, pts); +} + +double CarBounds2d::distToSide( int side, double maxDist, const vector& pts ) const +{ + CarBounds2d temp(*this); + + Vec2d midPt; + double midLen = 0; + switch( side ) + { + case SIDE_FRONT: + temp.pts[REAR_LFT] = temp.pts[FRNT_LFT]; + temp.pts[REAR_RGT] = temp.pts[FRNT_RGT]; + midPt = (temp.pts[FRNT_LFT] + temp.pts[FRNT_RGT]) * 0.5; + midLen = temp.pts[FRNT_LFT].dist(temp.pts[FRNT_RGT]); + break; + + case SIDE_REAR: + temp.pts[FRNT_LFT] = temp.pts[REAR_LFT]; + temp.pts[FRNT_RGT] = temp.pts[REAR_RGT]; + midPt = (temp.pts[REAR_LFT] + temp.pts[REAR_RGT]) * 0.5; + midLen = temp.pts[REAR_LFT].dist(temp.pts[REAR_RGT]); + break; + + case SIDE_LEFT: + temp.pts[FRNT_RGT] = temp.pts[FRNT_LFT]; + temp.pts[REAR_RGT] = temp.pts[REAR_LFT]; + midPt = (temp.pts[FRNT_LFT] + temp.pts[REAR_LFT]) * 0.5; + midLen = temp.pts[FRNT_LFT].dist(temp.pts[REAR_LFT]); + break; + + case SIDE_RIGHT: + temp.pts[FRNT_LFT] = temp.pts[FRNT_RGT]; + temp.pts[REAR_LFT] = temp.pts[REAR_RGT]; + midPt = (temp.pts[FRNT_RGT] + temp.pts[REAR_RGT]) * 0.5; + midLen = temp.pts[FRNT_RGT].dist(temp.pts[REAR_RGT]); + break; + } + + double filterDistSq = (midLen + maxDist) * (midLen + maxDist); + + if( temp.collidesWith(pts, midPt, filterDistSq) ) + return 0; + + temp.inflateSide( side, maxDist ); + if( !temp.collidesWith(pts, midPt, filterDistSq) ) + return maxDist; + + double incr = maxDist * 0.5; + double dist = maxDist - incr; + temp.inflateSide( side, -incr ); + + while( incr > 0.01 ) + { + if( temp.collidesWith(pts, midPt, filterDistSq) ) + { + incr *= 0.5; + dist -= incr; + temp.inflateSide( side, -incr ); + } + else + { + incr *= 0.5; + dist += incr; + temp.inflateSide( side, incr ); + } + } + + if( !temp.collidesWith(pts, midPt, filterDistSq) ) + dist -= incr; + + return dist; +} + +void CarBounds2d::inflateSide( int side, double delta ) +{ + switch( side ) + { + case SIDE_FRONT: + pts[FRNT_LFT] += xAxis * delta; + pts[FRNT_RGT] += xAxis * delta; + break; + + case SIDE_REAR: + pts[REAR_LFT] -= xAxis * delta; + pts[REAR_RGT] -= xAxis * delta; + break; + + case SIDE_LEFT: + pts[FRNT_LFT] += yAxis * delta; + pts[REAR_LFT] += yAxis * delta; + break; + + case SIDE_RIGHT: + pts[FRNT_RGT] -= yAxis * delta; + pts[REAR_RGT] -= yAxis * delta; + break; + } +} + +void CarBounds2d::inflate( double deltaX, double deltaY ) +{ + inflate( deltaX, deltaX, deltaY, deltaY ); +} + +void CarBounds2d::inflate( double deltaFront, double deltaRear, double deltaLeft, double deltaRight ) +{ + pts[FRNT_LFT] += xAxis * deltaFront + yAxis * deltaLeft; + pts[REAR_LFT] += -xAxis * deltaRear + yAxis * deltaLeft; + pts[REAR_RGT] += -xAxis * deltaRear - yAxis * deltaRight; + pts[FRNT_RGT] += xAxis * deltaFront - yAxis * deltaRight; +} + +bool CarBounds2d::contains( const Vec2d& pt ) const +{ + for( int i = 0; i < 4; i++ ) + { + if( Vec2d(pts[s_next_corner[i]] - pts[i]).GetNormal() * (pt - pts[i]) > 0 ) + return false; + } + + return true; +} + +bool CarBounds2d::collidesWith( const CarBounds2d& other ) const +{ + // test for points contained. + for( int i = 0; i < 4; i++ ) + { + if( contains(other.pts[i]) || other.contains(pts[i]) ) + return true; + } + + // test the edges crossing too. + double t1, t2; + for( int i = 0; i < 4; i++ ) + { + Vec2d v = pts[s_next_corner[i]] - pts[i]; + for( int j = 0; j < 4; j++ ) + { + Vec2d ov = other.pts[s_next_corner[j]] - other.pts[j]; + if( Utils::LineCrossesLine(pts[i], v, other.pts[j], ov, t1, t2) ) + { + if( t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1 ) + return true; + } + } + } + + return false; +} + +bool CarBounds2d::collidesWith( const Vec2d& pt1, const Vec2d& pt2 ) const +{ + double t1, t2; + Vec2d lv = pt2 - pt1; + + for( int i = 0; i < 4; i++ ) + { + Vec2d v = pts[s_next_corner[i]] - pts[i]; + if( Utils::LineCrossesLine(pts[i], v, pt1, lv, t1, t2) ) + { + if( t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1 ) + return true; + } + } + + return false; +} + +bool CarBounds2d::collidesWith( const std::vector& otherPts, const Vec2d& filterPt, double filterDistSqLimit ) const +{ + if( otherPts.empty() ) + return false; + + bool lastIsOk = filterDistSqLimit < 0 || otherPts[0].DistSq(filterPt) <= filterDistSqLimit; + + double t1, t2; + for( int i = 1; i < (int)otherPts.size(); i++ ) + { + bool currIsOk = filterDistSqLimit < 0 || otherPts[i].DistSq(filterPt) <= filterDistSqLimit; + + if( lastIsOk && currIsOk ) + { + Vec2d otherV = otherPts[i] - otherPts[i - 1]; + + for( int j = 0; j < 4; j++ ) + { + Vec2d v = pts[s_next_corner[j]] - pts[j]; + if( Utils::LineCrossesLine(otherPts[i - 1], otherV, pts[j], v, t1, t2) ) + { + if( t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1 ) + return true; + } + } + } + + lastIsOk = currIsOk; + } + + return false; +} diff --git a/src/drivers/shadow/src/CarBounds2d.h b/src/drivers/shadow/src/CarBounds2d.h new file mode 100644 index 000000000..2b9784ca2 --- /dev/null +++ b/src/drivers/shadow/src/CarBounds2d.h @@ -0,0 +1,45 @@ +#ifndef MOUSE_CAR_BOUNDARY_2D_ +#define MOUSE_CAR_BOUNDARY_2D_ + +#include "car.h" +#include "Vec2d.h" + +#include + +// The "SHADOW" logger instance. +extern GfLogger* PLogSHADOW; +#define LogSHADOW (*PLogSHADOW) + +class CarBounds2d +{ + Vec2d pts[4]; + Vec2d xAxis; // direction that is forwards. + Vec2d yAxis; // direction this is to the right. + +public: + enum + { + SIDE_FRONT, + SIDE_REAR, + SIDE_LEFT, + SIDE_RIGHT, + }; + +public: + CarBounds2d( const tCarElt* car ); + + const Vec2d& operator[]( int index ) const { return pts[index]; } + + double distToSide( int side, double maxDist, const CarBounds2d& other ) const; + double distToSide( int side, double maxDist, const std::vector& pts ) const; + + void inflateSide( int sideX, double delta ); + void inflate( double deltaX, double deltaY ); + void inflate( double deltaX1, double deltaX2, double deltaY1, double deltaY2 ); + bool contains( const Vec2d& pt ) const; + bool collidesWith( const CarBounds2d& other ) const; + bool collidesWith( const Vec2d& pt1, const Vec2d& pt2 ) const; + bool collidesWith( const std::vector& pts, const Vec2d& filterPt, double filterDistSqLimit = -1 ) const; +}; + +#endif // MOUSE_CAR_BOUNDARY_2D_ diff --git a/src/drivers/shadow/src/Driver.cpp b/src/drivers/shadow/src/Driver.cpp index 3523ac0c6..e945f6c44 100644 --- a/src/drivers/shadow/src/Driver.cpp +++ b/src/drivers/shadow/src/Driver.cpp @@ -770,10 +770,23 @@ void TDriver::NewRace( tCarElt* pCar, tSituation* pS ) LogSHADOW.debug("End Shadow NewRace\n"); } +bool TDriver::Pitting(int path, double pos) const +{ + return m_Strategy->needPitstop(car, m_Situation) && + m_pitPath[path].ContainsPos(pos); +} + +bool TDriver::Pitting(tCarElt* car) const +{ + double pos = m_track.CalcPos(car); + + return Pitting(PATH_NORMAL, pos); +} + void TDriver::GetPtInfo( int path, double pos, PtInfo& pi ) const { - if( m_Strategy->needPitstop(car, m_Situation) && m_pitPath[path].ContainsPos(pos) ) + if(m_Strategy->needPitstop(car, m_Situation) && m_pitPath[path].ContainsPos(pos) ) m_pitPath[path].GetPtInfo( pos, pi ); else m_path[path].GetPtInfo( pos, pi ); @@ -1858,6 +1871,19 @@ void TDriver::Drive( tSituation* s ) m_LastAccel = acc; m_LastAbsDriftAngle = m_AbsDriftAngle; + const Opponent::Sit& mySit = m_opp[car->index].GetInfo().sit; + m_stuck = NOT_STUCK; + + bool doStuckThing = true; + + if (Pitting(car)) + { + doStuckThing = false; + } + + if (doStuckThing) + m_stuckThing.execute(m_track, s, car, mySit); + m_Strategy->update(car, s); //m_Strategy->needPitstop(car, m_Situation); diff --git a/src/drivers/shadow/src/Driver.h b/src/drivers/shadow/src/Driver.h index 27562b5e3..a8f532c82 100644 --- a/src/drivers/shadow/src/Driver.h +++ b/src/drivers/shadow/src/Driver.h @@ -39,6 +39,7 @@ #include "LinePath.h" #include "PtInfo.h" #include "Strategy.h" +#include "Stuck.h" #include "teammanager.h" #define SECT_PRIV "private" @@ -189,6 +190,13 @@ public: void InitTrack(tTrack* track, void* carHandle, void** carParmHandle, tSituation* s); void NewRace(tCarElt* car, tSituation* s ); + void Drive(tSituation* s); + int PitCmd(tSituation* s); + void EndRace(tSituation* s); + void Shutdown(); + + bool Pitting(int path, double pos) const; + bool Pitting(tCarElt* car) const; void GetPtInfo( int path, double pos, PtInfo& pi ) const; void GetPosInfo( double pos, PtInfo& pi, double u, double v ) const; @@ -232,11 +240,6 @@ public: void SpeedControl6(double targetSpd, double spd0, CarElt* car, double& acc, double& brk ); void SpeedControl( int which, double targetSpd, double spd0, CarElt* car, double& acc, double& brk ); - void Drive(tSituation* s ); - int PitCmd(tSituation* s ); - void EndRace(tSituation* s ); - void Shutdown(); - double CurrSimTime; // Current simulation time double Frc; // Friction coefficient //bool UseBrakeLimit; // Enable/disable brakelimit @@ -358,6 +361,11 @@ private: MAX_OPP = 100 }; + enum StuckAction + { + NOT_STUCK, STUCK_GO_BACKWARDS, STUCK_GO_FORWARDS, + }; + private: MyTrack m_track; OptimisedPath m_path[N_PATHS]; @@ -481,19 +489,21 @@ private: double m_JumpOffset; // Offset for calculation of jumps bool m_FirstJump; int m_Flying; // Flag prepare landing - int m_nCars; - int m_myOppIdx; - Opponent *m_opp; // info about other cars. - double m_avgAY; - bool m_raceStart; - double m_avoidS; // where we are LR->T (0..1). - double m_avoidSVel; - double m_avoidT; // where we are L->R (-1..1). - double m_avoidTVel; - double m_avoidU; - double m_avoidV; - double m_attractor; // where we want to be. - int m_followPath; // path we want to follow; + int m_nCars; + int m_myOppIdx; + Opponent *m_opp; // info about other cars. + double m_avgAY; + bool m_raceStart; + double m_avoidS; // where we are LR->T (0..1). + double m_avoidSVel; + double m_avoidT; // where we are L->R (-1..1). + double m_avoidTVel; + double m_avoidU; + double m_avoidV; + double m_attractor; // where we want to be. + int m_followPath; // path we want to follow; + Stuck m_stuckThing; + StuckAction m_stuck; LinearRegression m_accBrkCoeff; // double m_brkCoeff[50]; diff --git a/src/drivers/shadow/src/PtInfo.h b/src/drivers/shadow/src/PtInfo.h index 9c1261533..d5d3b8e34 100644 --- a/src/drivers/shadow/src/PtInfo.h +++ b/src/drivers/shadow/src/PtInfo.h @@ -32,6 +32,8 @@ public: double oang; // global angle. double toL; // distance to edge of track on left. double toR; // distance to edge of track on right. + double extL; + double extR; double k; // curvature at point. double kz; // curvature in z at point double spd; // speed. diff --git a/src/drivers/shadow/src/Strategy.cpp b/src/drivers/shadow/src/Strategy.cpp index 4de10cbb1..8889ad8b9 100644 --- a/src/drivers/shadow/src/Strategy.cpp +++ b/src/drivers/shadow/src/Strategy.cpp @@ -47,9 +47,11 @@ SimpleStrategy::SimpleStrategy() fuelPerLap = 0.0f; // [Kg] The maximum amount of fuel we needed for a lap. lastPitFuel = 0.0f; // [Kg] Amount refueled, special case when we refuel. fuelSum = 0.0f; // [Kg] All the fuel used. + fuelSumPerMeter = 0.0; counterFuelLaps = 0; // [-] Counter of the total laps. countPitStop = 0; // [-] Counter of the total pitStop. avgFuelPerLap = 0.0; // [Kg] Average fuel per lap. + avgFuelPerMeter = 0.0; m_maxDamage = 0; // [-] max damage before we request a pit stop. m_Fuel = 0; // [Kg] Security fuel at the strat race. m_expectedfuelperlap = 0; // [Kg] Expected fuel per lap @@ -78,7 +80,7 @@ void SimpleStrategy::setFuelAtRaceStart(tTrack* t, void **carParmHandle, tSituat maxFuel = (tdble)(GfParmGetNum(*carParmHandle, SECT_CAR, PRM_TANK, (char*)NULL, (tdble) MAX_FUEL_TANK)); LogSHADOW.info("Strategy max fuel = %.2f\n", maxFuel); FuelperMeters = GfParmGetNum(*carParmHandle, SECT_PRIV, PRV_FUELPERMETERS, (char*)NULL, (tdble) MAX_FUEL_PER_METER); - LogSHADOW.info("Strategy fuel per meters = %.5f\n", FuelperMeters); + LogSHADOW.info("Strategy fuel per meters = %.7f\n", FuelperMeters); fuelPerLap = (tdble) (TrackLength * FuelperMeters); LogSHADOW.info("Strategy fuel per lap = %.2f\n", fuelPerLap); fullfuel = GfParmGetNum(*carParmHandle, SECT_PRIV, PRV_FULL_FUEL, (char*)NULL, 0.0); @@ -161,14 +163,14 @@ void SimpleStrategy::setFuelAtRaceStart(tTrack* t, void **carParmHandle, tSituat m_fuel = 0; //m_FuelStart = ((raceLaps / 2) * fuelPerLap) + 0.3; m_FuelStart = (tdble)(1.92 * fuelPerLap); - fprintf(stderr,"...Check PitStop Enabled!\n"); + LogSHADOW.info("...Check PitStop Enabled!\n"); //maybe we need to check Best Lap Time for qualifying } else if (test_qualifTime && s->_raceType == RM_TYPE_PRACTICE) { qualifRace = true; m_fuel = 0; m_FuelStart = s->_totLaps * fuelPerLap; - fprintf(stderr,"...Check QualifTime Enabled!\n"); + LogSHADOW.info("...Check QualifTime Enabled!\n"); } if (m_FuelStart > maxFuel) @@ -185,7 +187,7 @@ void SimpleStrategy::setFuelAtRaceStart(tTrack* t, void **carParmHandle, tSituat m_FuelStart = (tdble)(MIN(m_FuelStart, maxFuel)); - fprintf(stderr,"# SHADOW Index %d : Laps = %d, Fuel per Lap = %.2f, securityFuel = + %.2f, Fuel at Start Race = %.2f\n", + LogSHADOW.debug("# SHADOW Index %d : Laps = %d, Fuel per Lap = %.2f, securityFuel = + %.2f, Fuel at Start Race = %.2f\n", index, s->_totLaps, fuelPerLap, m_fuel, m_FuelStart); GfParmSetNum(*carParmHandle, SECT_CAR, PRM_FUEL, (char*)NULL, m_FuelStart); @@ -203,9 +205,14 @@ void SimpleStrategy::update(tCarElt* car, tSituation *s) { //fuelPerLap = MAX(fuelPerLap, (lastFuel + lastPitFuel - car->priv.fuel)); fuelSum += (lastFuel + lastPitFuel - car->priv.fuel); + fuelSumPerMeter += (lastFuel - car->_fuel); fuelPerLap = (fuelSum/(car->race.laps - 1)); counterFuelLaps++; avgFuelPerLap = fuelSum / counterFuelLaps; + avgFuelPerMeter = fuelSumPerMeter / car->_distRaced; + + LogSHADOW.info(" # Fuel per meter = %.7f\n", avgFuelPerMeter); + if (strategy_verbose) { @@ -290,7 +297,7 @@ bool SimpleStrategy::needPitstop(tCarElt* car, tSituation *s) { if (!m_checkFuel) { - LogSHADOW.debug("%s Go to Pit the next lap to refuel: reqFuel= %.2f, carFuel= %.2f, remLap= %d\n", + LogSHADOW.info("%s Go to Pit the next lap to refuel: reqFuel= %.2f, carFuel= %.2f, remLap= %d\n", car->_name, reqfuel, car->_fuel, car->_remainingLaps); m_checkFuel = true; } @@ -407,7 +414,8 @@ float SimpleStrategy::pitRefuel(tCarElt* car, tSituation *s) { LogSHADOW.debug(">> [PitStrat 2] "); } - LogSHADOW.debug(" %s in Pit to refuel < PitStop %d >\n", car->_name, countPitStop); + + LogSHADOW.info(" %s in Pit to refuel < PitStop %d >\n", car->_name, countPitStop); m_fuelperstint = (tdble)((fuelToEnd / num_remStops) + addFuel); @@ -434,13 +442,14 @@ float SimpleStrategy::pitRefuel(tCarElt* car, tSituation *s) if (m_remainingstops >= 1) { m_fuelperstint = (tdble)((fuelToEnd / num_remStops) + addFuel); - } else if (m_remainingstops <= 0) + } + else if (m_remainingstops <= 0) { m_fuelperstint = (tdble)((lapsToEnd * fuelPerLap) + addMinFuel); } fuel = MIN(m_fuelperstint - car->_fuel, car->_tank - car->_fuel); - LogSHADOW.debug(" %s in Pit to refuel %.2fl < PitStop %d >\n", car->_name, fuel, countPitStop); + LogSHADOW.info(" %s in Pit to refuel %.2fl < PitStop %d >\n", car->_name, fuel, countPitStop); } diff --git a/src/drivers/shadow/src/Strategy.h b/src/drivers/shadow/src/Strategy.h index 226291d65..89daa2b17 100644 --- a/src/drivers/shadow/src/Strategy.h +++ b/src/drivers/shadow/src/Strategy.h @@ -134,9 +134,11 @@ protected: float lastPitFuel; // Amount refueled, special case when we refuel. float lastFuel; // The fuel available when we cross the start lane. float fuelSum; // All the fuel used needed for the race. + double fuelSumPerMeter; int counterFuelLaps; // Counter of the total laps. int countPitStop; // Counter of the total pits stop. double avgFuelPerLap; // The average amount of fuel we needed for a lap. + double avgFuelPerMeter; int m_maxDamage; // The Max damage set in the XML file. float m_Fuel; // The new average fuel per lap float m_expectedfuelperlap; // Expected fuel per lap (may be very inaccurate). diff --git a/src/drivers/shadow/src/Stuck.cpp b/src/drivers/shadow/src/Stuck.cpp new file mode 100644 index 000000000..1d438eabb --- /dev/null +++ b/src/drivers/shadow/src/Stuck.cpp @@ -0,0 +1,1417 @@ +#include +#include +#include + +#include "Stuck.h" +#include "CarBounds2d.h" + +using namespace std; + +const int Stuck::delta8_x[8] = {1, 1, 0, -1, -1, -1, 0, 1}; +const int Stuck::delta8_y[8] = {0, 1, 1, 1, 0, -1, -1, -1}; +const float Stuck::delta8_t[8] = {1.0f/Stuck::SPD, 1.0f/Stuck::SPD * 1.414f, + 1.0f/Stuck::SPD, 1.0f/Stuck::SPD * 1.414f, + 1.0f/Stuck::SPD, 1.0f/Stuck::SPD * 1.414f, + 1.0f/Stuck::SPD, 1.0f/Stuck::SPD * 1.414f}; + +const float Stuck::delta64_t[64] = { +/* + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.004f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.045f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.134f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.294f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.294f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.134f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.045f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.004f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.004f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.045f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.134f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.294f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.294f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.134f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.045f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.004f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.004f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.045f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.134f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.294f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.294f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.134f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.045f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.004f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.004f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.045f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.134f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.294f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.294f, 1.0f/Stuck::SPD * 1.203f, 1.0f/Stuck::SPD * 1.134f, + 1.0f/Stuck::SPD * 1.082f, 1.0f/Stuck::SPD * 1.045f, 1.0f/Stuck::SPD * 1.020f, 1.0f/Stuck::SPD * 1.004f, +*/ +/* + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.500f, + 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.050f, +*/ + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.150f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.480f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.440f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.440f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.480f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.150f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.150f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.480f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.440f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.440f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.480f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.150f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.150f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.480f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.440f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.440f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.480f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.150f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.050f, + + 1.0f/Stuck::SPD * 1.000f, 1.0f/Stuck::SPD * 1.050f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.150f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.480f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.440f, + 1.0f/Stuck::SPD * 1.414f, 1.0f/Stuck::SPD * 1.440f, 1.0f/Stuck::SPD * 1.460f, 1.0f/Stuck::SPD * 1.480f, + 1.0f/Stuck::SPD * 1.500f, 1.0f/Stuck::SPD * 1.150f, 1.0f/Stuck::SPD * 1.100f, 1.0f/Stuck::SPD * 1.050f, +}; + +Stuck::Stuck() +: _me(0), + _stuckState(RACING), + _stuckTime(0), + _stuckCount(0) +{ + _grid.resize( GRID_SIZE ); + for( int x = 0; x < (int)_grid.size(); x++ ) + _grid[x].resize( GRID_SIZE ); +} + +Stuck::~Stuck() +{ +} + +// racing. +// normal racing. no control applied here, just monitoring the +// car's situation to decide if its stuck or needs re-orientation. +// +// if angle not OK, go to reorient. +// if halted for too long, go to stuck. +// +// reorient. +// car angle more than 30 degrees to racing line. use forwards and backwards movements +// with steering to try to reorient the car. if halted for too long go to stuck. +// +// stuck. +// car has come to a halt. start rearching for a plan to get unstuck. +// once a plan is found, start to execute it. +// +// execute plan. +// follow plan, monitoring the car situation. if stuck again, go back to stuck. +// once free, go back to racing. +// + +bool Stuck::execute( const MyTrack& track, const tSituation* s, tCarElt* me, const Opponent::Sit& mySit ) +{ +// PRINTF( "stuck %d stuck_time %1.2f speed %1.2f\n", +// _stuckState, _stuckTime, me->_speed_x ); + + double start = GfTimeClock(); + + switch( _stuckState ) + { + case RACING: + executeRacing( track, s, me, mySit ); + break; + + case REORIENT_FORWARDS: + case REORIENT_BACKWARDS: + executeReorient( track, s, me, mySit ); + break; + + case REINIT: + executeInit( track, s, me ); + break; + + case SOLVING: + executeSolving( track, s, me ); + break; + + case EXEC_PLAN: + executePlan( track, s, me ); + break; + } + + double elapsed = GfTimeClock() - start; +// PRINTF( "[%d] stuck CPU time: %0.6f seconds.\n", me->index, elapsed ); + + return _stuckState == EXEC_PLAN; +} + +void Stuck::executeRacing( const MyTrack& track, const tSituation* s, const tCarElt* me, const Opponent::Sit& mySit ) +{ + updateStuckTime( me, s ); + + double dirAng = mySit.pi.oang - me->_yaw; + NORM_PI_PI(dirAng); + + if( fabs(dirAng) > 30 * PI / 180 ) + reorient( me, dirAng ); + + if( _stuckTime < 1 ) + return; + + init( track, s, me ); +} + +void Stuck::executeReorient( const MyTrack& track, const tSituation* s, tCarElt* me, const Opponent::Sit& mySit ) +{ + LogSHADOW.debug( "[%d] reorient. rev count %d\n", me->index, _stuckCount ); + + updateStuckTime( me, s ); + + double dirAng = mySit.pi.oang - me->_yaw; + NORM_PI_PI(dirAng); + + if( fabs(dirAng) < 30 * PI / 180 ) + { + _stuckState = RACING; + LogSHADOW.debug( "[%d] reorient. finished.\n", me->index ); + return; + } + + if( _stuckCount > 10 ) + { + // give up... use the solver instead. + LogSHADOW.debug( "[%d] reorient. start solvers.\n", me->index ); + _stuckState = REINIT; + _stuckCount = 0; + _stuckTime = 0.0; + return; + } + + double sideAhead = dirAng > 0 ? mySit.pi.extR + me->pub.trkPos.toMiddle : mySit.pi.extL - me->pub.trkPos.toMiddle; + double sideBehind = dirAng > 0 ? mySit.pi.extL - me->pub.trkPos.toMiddle : mySit.pi.extR + me->pub.trkPos.toMiddle; + + double distAhead = 25; + double distBehind = 25; + CarBounds2d bounds(me); + + for( int i = 0; i < s->raceInfo.ncars; i++ ) + { + const tCarElt* oCar = s->cars[i]; + if( oCar == me || (oCar->pub.state & RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT) != 0 ) + continue; + + const CarBounds2d oBounds(oCar); + + distAhead = bounds.distToSide(CarBounds2d::SIDE_FRONT, distAhead, oBounds); + distBehind = bounds.distToSide(CarBounds2d::SIDE_REAR, distBehind, oBounds); + //PRINTF( "[%d] [%d] distAhead %g, distBehind %g\n", car->index, oCar->index, distAhead, distBehind ); + } + + int gear = 1; + double brk = 0; + + switch( _stuckState ) + { + case REORIENT_BACKWARDS: + gear = -1; + brk = me->pub.DynGC.vel.x > 0 ? 0.5 : 0; + if( distBehind < 0.2 || sideBehind < 2.5 ) + { + LogSHADOW.debug( "[%d] reorient go forwards\n", me->index ); + _stuckCount++; + _stuckState = REORIENT_FORWARDS; + _stuckTime = 0; + } + break; + + case REORIENT_FORWARDS: + brk = me->pub.DynGC.vel.x < 0 ? 0.5 : 0; + if( distAhead < 0.2 || sideAhead < 2.5 ) + { + LogSHADOW.debug( "[%d] reorient go backwards\n", me->index ); + _stuckCount++; + _stuckState = REORIENT_BACKWARDS; + _stuckTime = 0; + } + break; + } + + double steer = 0; + if( me->_speed_x < 0 ) + steer = -SGN(dirAng); + else + steer = SGN(dirAng); + + double acc = fabs(me->_speed_x) < 3 ? 1.0 : 0.2; + + double w1_speed = me->priv.wheel[0].spinVel * me->info.wheel[0].wheelRadius; + double w2_speed = me->priv.wheel[1].spinVel * me->info.wheel[1].wheelRadius; + double w3_speed = me->priv.wheel[2].spinVel * me->info.wheel[2].wheelRadius; + double w4_speed = me->priv.wheel[3].spinVel * me->info.wheel[3].wheelRadius; + + double front_speed = (w1_speed + w2_speed) * 0.5; + if( gear > 0 && (w3_speed > front_speed + 2 || w4_speed > front_speed + 2) || + gear < 0 && (w3_speed < front_speed - 2 || w4_speed < front_speed - 2) ) + acc = 0.1; + + me->ctrl.steer = steer; + me->ctrl.gear = gear; + me->ctrl.accelCmd = acc; + me->ctrl.brakeCmd = brk; +} + +void Stuck::executeInit( const MyTrack& track, const tSituation* s, tCarElt* me ) +{ + if( _stuckTime > 0 ) + _stuckTime -= s->deltaTime; + else + init( track, s, me ); + + me->ctrl.steer = 0; + me->ctrl.accelCmd = 0; + me->ctrl.brakeCmd = 1; +} + +void Stuck::executeSolving( const MyTrack& track, const tSituation* s, tCarElt* me ) +{ +// updateStuckTime( me, s ); + +/* if( opponentsChanged(s, me) ) + { + init( track, s, me ); + return; + }*/ + if( clearAhead(track, s, me) ) + { + _stuckState = RACING; + return; + } + + me->ctrl.accelCmd = 0; + me->ctrl.brakeCmd = 1; + +// if( !solve(me) ) + if( !solveR(me) ) + { + // no solution possible with initial state... try again in a short time. + _stuckCount++; + LogSHADOW.debug( "stuck: [%d] No solution: re-initting.\n", _stuckCount ); + _stuckState = _stuckCount < 10 ? REINIT : RACING; + _stuckTime = 0.09; + } +} + +void Stuck::executePlan( const MyTrack& track, const tSituation* s, tCarElt* me ) +{ +/* updateStuckTime( me, s ); + + if( _stuckTime > 1 ) + { + init( track, s, me ); + return; + }*/ + +/* if( opponentsChanged(s, me) ) + { + init( track, s, me ); + return; + }*/ + + if( clearAhead(track, s, me) ) + { + _stuckState = RACING; + return; + } + + getUnstuck( track, me, s ); +} + +void Stuck::updateStuckTime( const tCarElt* me, const tSituation* s ) +{ + if( fabs(me->_speed_x) > 2 || s->currentTime < 0 ) + _stuckTime = 0; + else + _stuckTime += s->deltaTime; +} + +void Stuck::sort( vector& row, int y ) +{ + for( int i = 0; i < (int)row.size(); i++ ) + row[i].x = row[i].calcX(y); + + std::sort( row.begin(), row.end() ); +} + +void Stuck::reorient( const tCarElt* me, double dirAng ) +{ + _stuckCount = 0; + _stuckState = dirAng * me->_trkPos.toMiddle < 0 ? + REORIENT_BACKWARDS : REORIENT_FORWARDS; + _stuckTime = 0; +} + +void Stuck::makeOpponentsList( const tSituation* s, const tCarElt* me, vector* opponents ) +{ + opponents->clear(); + + for( int i = 0; i < s->raceInfo.ncars; i++ ) + { + const tCarElt* other = s->cars[i]; + if( other->index == me->index || (other->pub.state & RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT) ) + continue; + + double carX = other->pub.DynGCg.pos.x - _gridOrigin.x; + double carY = other->pub.DynGCg.pos.y - _gridOrigin.y; + + int intCarX = (int)floor(carX + 0.5); + int intCarY = (int)floor(carY + 0.5); + + if( other->pub.speed > 2 || + carX < 0 || carX >= GRID_SIZE || + carY < 0 || carY >= GRID_SIZE ) + continue; // ignore cars that aren't stopped, or are outside of our grid area. + + opponents->push_back( OppInfo(carX, carY, other) ); + } +} + +bool Stuck::clearAhead( const MyTrack& track, const tSituation* s, const tCarElt* me ) const +{ +// const MyTrack::Seg& seg(track.GetAt(track.IndexFromPos(me->race.distFromStartLine))); + double width = track.GetWidth(); + double offs = -me->pub.trkPos.toMiddle; + LogSHADOW.debug( "offs=%.2f width=%.2f\n", offs, width ); + if( offs < -width / 2 || offs > width / 2 ) + return false; // still not back onto main raceway. + + for( int i = 0; i < s->raceInfo.ncars; i++ ) + { + const tCarElt* other = s->cars[i]; + if( other->index == me->index || (other->pub.state & RM_CAR_STATE_NO_SIMU) ) + continue; + + int carX = (int)floor(other->pub.DynGCg.pos.x - _gridOrigin.x + 0.5); + int carY = (int)floor(other->pub.DynGCg.pos.y - _gridOrigin.y + 0.5); + + if( other->pub.speed > 2 || + carX < 0 || carX >= GRID_SIZE || + carY < 0 || carY >= GRID_SIZE ) + continue; // ignore cars that aren't stopped, or are outside of our grid area. + + double relPos = other->race.distFromStartLine - me->race.distFromStartLine; + if( relPos > track.GetLength() / 2 ) + relPos -= track.GetLength(); + else if( relPos < -track.GetLength() / 2 ) + relPos += track.GetLength(); + + if( relPos > 0 ) + return false; + } + + return true; +} + +bool Stuck::opponentsChanged( const tSituation* s, const tCarElt* me ) +{ + vector opponents; + makeOpponentsList( s, me, &opponents ); + return _opponents != opponents; +} + +void Stuck::init( const MyTrack& track, const tSituation* s, const tCarElt* me ) +{ + LogSHADOW.debug( "[%d] stuck::init\n", me->index ); + LogSHADOW.debug( "[%d] len %g steer lock %g\n", + me->index, me->priv.wheel[2].relPos.x - me->priv.wheel[0].relPos.x, + me->info.steerLock ); + + _me = me; + _gridOrigin = Vec2d(me->pub.DynGCg.pos.x - GRID_RAD, me->pub.DynGCg.pos.y - GRID_RAD); + _stuckState = RACING; + _plan.clear(); + + // clear all cells. + for( int x = 0; x < GRID_SIZE; x++ ) + for( int y = 0; y < GRID_SIZE; y++ ) + _grid[x][y].clear(); + + fillTrackCells( track ); + + const tCarElt* aheadCar = NULL; + const tCarElt* behindCar = NULL; + + // make opponent car cells unavailable. + makeOpponentsList( s, me, &_opponents ); + + for( int i = 0; i < _opponents.size(); i++ ) + { + const OppInfo& oppInfo = _opponents[i]; + const tCarElt* other = oppInfo.car; + + if( aheadCar == NULL || + aheadCar->race.distFromStartLine < other->race.distFromStartLine ) + aheadCar = other; + + if( behindCar == NULL || + behindCar->race.distFromStartLine > other->race.distFromStartLine ) + behindCar = other; + + fillCarCells( i, oppInfo.x, oppInfo.y, other->pub.DynGC.pos.az, 2.25, 1.0, 1.0, true ); + } + + fillCarCells( -1, 0, 0, me->pub.DynGC.pos.az, 1.5, 0.5, 0.0, false ); + + double width = track.GetWidth(); + double offs = -me->pub.trkPos.toMiddle; + LogSHADOW.debug( "offs=%.2f width=%.2f\n", offs, width ); +// if( offs < -width / 2 || offs > width / 2 ) +// return false; // still not back onto main raceway. + bool onMainRaceway = offs > -width / 2 && offs < width / 2; + + if( onMainRaceway && + (aheadCar == NULL || + aheadCar->race.distFromStartLine + 2 < me->race.distFromStartLine) ) + { + // no cars ahead are in the way so go back to racing. + _stuckState = RACING; + _stuckTime = 0; + return; + } + + double aheadCutOff = 7; + + if( aheadCar == NULL || + aheadCar->race.distFromStartLine < me->race.distFromStartLine ) + { + aheadCar = me; + aheadCutOff = 15; // stuck even though there's no other car ahead, so probably + // stuck in the scenery. add some more space to make sure to + // navigate around it. + } + + if( behindCar == NULL || + behindCar->race.distFromStartLine > me->race.distFromStartLine ) + behindCar = me; + + // figure out position ahead for destination (and cut-off). + LogSHADOW.debug( "my car: %g\n", me->race.distFromStartLine ); + LogSHADOW.debug( "car ahead: %g\n", aheadCar->race.distFromStartLine ); + double aheadPos = aheadCar->race.distFromStartLine + aheadCutOff; + if( aheadPos > me->race.distFromStartLine + GRID_RAD * 9 / 10 ) + aheadPos = me->race.distFromStartLine + GRID_RAD * 9 / 10; + LogSHADOW.debug( "ahead pos: %g\n", aheadPos ); + + // figure out position behind for cut-off. + LogSHADOW.debug( "car behind: %g\n", behindCar->race.distFromStartLine ); + double behindPos = behindCar->race.distFromStartLine - 10; + if( behindPos < me->race.distFromStartLine - GRID_RAD * 9 / 10 ) + behindPos = me->race.distFromStartLine - GRID_RAD * 9 / 10; + LogSHADOW.debug( "behind pos: %g\n", behindPos ); + + // draw lines ahead and behind to help limit the search. + int aheadSegIndex = (track.IndexFromPos(aheadPos) + 1) % track.GetSize(); + const Seg& aheadCutSeg = track.GetAt(aheadSegIndex); + int lastX = -1; + for( double offs = -aheadCutSeg.el - 1.5; offs < aheadCutSeg.er + 1.5; offs += 0.5 ) + { + Vec3d pt = aheadCutSeg.pt + offs * aheadCutSeg.norm; + + int x = (int)floor(pt.x - _gridOrigin.x); + int y = (int)floor(pt.y - _gridOrigin.y); + if( !isValid(x, y) ) + continue; + + Cell& cell = at(x, y); + cell.addEdgeMask(); + + if( lastX >= 0 ) + { + Cell& cell2 = at(lastX, y); + cell2.addEdgeMask(); + } + + lastX = x; + } + + const Seg& behindCutSeg = track.GetAt(track.IndexFromPos(behindPos)); + lastX = -1; + for( double offs = -behindCutSeg.el - 1.5; offs < behindCutSeg.er + 1.5; offs += 0.5 ) + { + Vec3d pt = behindCutSeg.pt + offs * behindCutSeg.norm; + + int x = (int)floor(pt.x - _gridOrigin.x); + int y = (int)floor(pt.y - _gridOrigin.y); + if( !isValid(x, y) ) + continue; + + Cell& cell = at(x, y); + cell.addEdgeMask(); + + if( lastX >= 0 ) + { + Cell& cell2 = at(lastX, y); + cell2.addEdgeMask(); + } + + lastX = x; + } + + GridPoint car_pt1(*this, me, true, 0); + GridPoint car_pt2(car_pt1.x(), car_pt1.y(), car_pt1.iang(), false, 0, 0); + _origCarPt = car_pt1; + + // invalidate bad directions in cells. + const int carx = car_pt1.x(); + const int cary = car_pt1.y(); + const int cariang = car_pt1.iang(); + for( int y = 0; y < GRID_SIZE; y++ ) + { + for( int x = 0; x < GRID_SIZE; x++ ) + { + Cell& cell = at(x, y); + if( !cell.isAvailable() ) + continue; + + for( int octang = 0; octang < 8; octang++ ) + { + int dx = delta8_x[octang]; + int dy = delta8_y[octang]; + int iang = 8 * octang; + + if( !isValid(x + dx, y + dy) || !at(x + dx, y + dy).isAvailable() || + !isValid(x - dx, y - dy) || !at(x - dx, y - dy).isAvailable()) + { + for( int ang = iang - 4; ang < iang + 4; ang++ ) + { + if( x == carx && y == cary && (ang & 0x3F) == cariang ) + { + // don't invalidate where the car actually is. + continue; + } + + cell.times[fwang(ang & 0x3F, true)] = -1; + cell.times[fwang(ang & 0x3F, false)] = -1; + } + } + } + } + } + + // fill in distances from cars and walls. + deque dq; + for( int x = 0; x < GRID_SIZE; x++ ) + for( int y = 0; y < GRID_SIZE; y++ ) + if( !at(x, y).isAvailable() ) + dq.push_back(GridPoint(x, y, 0, false, 0, 0)); + + while( !dq.empty() ) + { + GridPoint pt = dq.front(); + dq.pop_front(); + + int x = pt.x(); + int y = pt.y(); + Cell& from = at(x, y); + for( int a = 0; a < 8; a += 2 ) // += 2 to only do orthogonal directions. + { + int dx = delta8_x[a]; + int dy = delta8_y[a]; + float dt = delta8_t[a]; + + int x2 = x + dx; + int y2 = y + dy; + if( !isValid(x2, y2) ) + continue; + + Cell& cell = at(x2, y2); + if( !cell.isAvailable() ) + continue; + + if( cell.dist_from_walls == 0 ) + { + cell.dist_from_walls = from.dist_from_walls + 1; + dq.push_back( GridPoint(x2, y2, 0, false, 0, 0) ); + } + } + } + + // fill in estimates to car. + at(car_pt1).est_time_to_car = 0; + at(car_pt2).est_time_to_car = 0; + dq.push_back( car_pt1 ); + while( !dq.empty() ) + { + GridPoint pt = dq.front(); + dq.pop_front(); + + int x = pt.x(); + int y = pt.y(); + for( int a = 0; a < 8; a++ ) + { + int dx = delta8_x[a]; + int dy = delta8_y[a]; + float dt = delta8_t[a]; + + int x2 = x + dx; + int y2 = y + dy; + + Cell& cell = at(x2, y2); + if( !cell.isAvailable() ) + continue; + + if( cell.est_time_to_car < 0 ) + { + float est = pt.est_time + dt; + cell.est_time_to_car = est; + dq.push_back( GridPoint(x2, y2, a, true, 0, est) ); + } + } + } + + // fill in estimates to dest. + const Seg& destSeg = track.GetAt(track.IndexFromPos(aheadPos)); + int mid_ang = to_iang(destSeg.norm.GetXY().GetAngle() + PI / 2); + _destinations.clear(); + for( double offs = -destSeg.wl; offs < destSeg.wr; offs += 0.5 ) + { + Vec3d pt = destSeg.pt + offs * destSeg.norm; + int x = (int)floor(pt.x - _gridOrigin.x); + int y = (int)floor(pt.y - _gridOrigin.y); + + if( !isValid(x, y) ) + continue; + + Cell& cell = at(x, y); + + if( !cell.isAvailable() ) + continue; + + cell.est_time_to_dest = 0; + GridPoint dest = GridPoint(x, y, mid_ang, true, 0, 0); + cell.setSolution( dest.fwang() ); + _destinations.push_back( dest ); + dq.push_back( dest ); + } + while( !dq.empty() ) + { + GridPoint pt = dq.front(); + dq.pop_front(); + + int x = pt.x(); + int y = pt.y(); + for( int a = 0; a < 8; a++ ) + { + int dx = delta8_x[a]; + int dy = delta8_y[a]; + float dt = delta8_t[a]; + + int x2 = x + dx; + int y2 = y + dy; + + Cell& cell = at(x2, y2); + if( !cell.isAvailable() ) + continue; + + if( cell.est_time_to_dest < 0 ) + { + float est = pt.est_time + dt; + cell.est_time_to_dest = est; + dq.push_back( GridPoint(x2, y2, a, true, 0, est) ); + } + } + } + + // if the car position doesn't have an estimate to the destination + // then the car is trapped. no need to search, just re-init in a + // short time in the hope something has changed. + if( at(car_pt1).est_time_to_dest < 0 && at(car_pt2).est_time_to_dest < 0 ) + { +// dumpGrid(); + LogSHADOW.info( "[%d] stuck::init -- no solution -- reinit.\n", me->index ); +// _stuckState = REINIT; +// _stuckTime = 0.09; + _stuckState = RACING; + _stuckTime = 0.0; + return; + } + + // initialise priority queue with destination positions. +// _pqN.clear(); + _pqN = priority_queue(); + for( double offs = -destSeg.wl; offs < destSeg.wr; offs += 0.5 ) + { + // mark all distinations as 0 time (which flags them as having been visited.) + Vec3d pt = destSeg.pt + offs * destSeg.norm; + int x = (int)floor(pt.x - _gridOrigin.x); + int y = (int)floor(pt.y - _gridOrigin.y); + + if( !isValid(x, y) ) + continue; + + Cell& cell = at(x, y); + + if( !cell.isAvailable() ) + continue; + + int mid_ang = to_iang(destSeg.norm.GetXY().GetAngle() + PI / 2); + if( cell.times[fwang(mid_ang, true)] > 0 ) + { +// for( int a = -OCTANT; a <= OCTANT; a++ ) + for( int a = -1; a <= 1; a++ ) + { + int iang = (mid_ang + N_ANGLES + a) % N_ANGLES; + //cell.times[fwang(iang, true)] = 0; + _pqN.push( GridPoint(x, y, iang, true, 0, cell.est_time_to_car) ); + } + } + } + + // initialise priority queue with car start positions. + _pqR = priority_queue(); +// for( int i = -2; i <= 2; i++ ) +// for( int i = -1; i <= 1; i++ ) + for( int i = 0; i <= 0; i++ ) + { + int iang = (car_pt1.iang() + i) & 63; + _pqR.push( GridPoint(car_pt1.x(), car_pt1.y(), iang, car_pt1.fw(), 0, at(car_pt1).est_time_to_dest + abs(i) * 2.0) ); + _pqR.push( GridPoint(car_pt2.x(), car_pt2.y(), iang, car_pt2.fw(), 0, at(car_pt2).est_time_to_dest + abs(i) * 2.0) ); + } + + // initialise other fields. + _expansionsN = 0; + _expansionsR = 0; + _bestTime = 9e9f; + _stuckState = SOLVING; + + dumpGrid(); + + LogSHADOW.info( "[%d] stuck::init -- done\n", me->index ); +} + +void Stuck::fillCarCells( int carI, double carX, double carY, double carAng, double len, double wid, double rad, bool addMask ) +{ + int minX = MX(0, MN((int)floor(carX - CAR_RAD), GRID_SIZE - 1)); + int minY = MX(0, MN((int)floor(carY - CAR_RAD), GRID_SIZE - 1)); + int maxX = MX(0, MN((int)ceil( carX + CAR_RAD), GRID_SIZE - 1)); + int maxY = MX(0, MN((int)ceil( carY + CAR_RAD), GRID_SIZE - 1)); + + double vx = cos(carAng); + double vy = sin(carAng); + + // work out enclosing rectangle car coordinates. + double rx = len + rad; + double ry = wid + rad; + + for( int x = minX; x <= maxX; x++ ) + { + for( int y = minY; y <= maxY; y++ ) + { + if( x == GRID_RAD && y == GRID_RAD ) + { + // ignore cell containing my car + continue; + } +/* + int dx = x - carX; + int dy = y - carY; + if( dx*dx + dy*dy <= CAR_RAD * CAR_RAD ) + { + _grid[x][y].addCarMask(i); + } +*/ + double dx = vx * (x - carX) + vy * (y - carY); + double dy = -vy * (x - carX) + vx * (y - carY); + if( fabs(dx) > rx || fabs(dy) > ry ) + { + // pt not inside rectangle around car + continue; + } + + // now check the corners? + if( rad ) + { + double cx = fabs(dx) - len; + double cy = fabs(dy) - wid; + if( cx > 0 && cy > 0 ) + { + if( cx * cx + cy * cy > rad * rad ) + { + // point is outside of the rounded corners. + continue; + } + } + } + + if( addMask ) + _grid[x][y].addCarMask(carI); + else + _grid[x][y].clearAllCarMasks(); + } + } +} + +void Stuck::fillTrackCells( const MyTrack& track ) +{ + // make track cells available. + vector> edges(GRID_SIZE); + + _leftPoints.clear(); + _rightPoints.clear(); + + const double margin = 0.5; + const int NSEG = track.GetSize(); + const Seg& lastSeg = track.GetAt(NSEG - 1); + Vec3d l1 = lastSeg.pt - (lastSeg.el - margin) * lastSeg.norm; + Vec3d r1 = lastSeg.pt + (lastSeg.er - margin) * lastSeg.norm; + + _leftPoints.push_back( lastSeg.pt.GetXY() - lastSeg.el * lastSeg.norm.GetXY() ); + _rightPoints.push_back( lastSeg.pt.GetXY() + lastSeg.er * lastSeg.norm.GetXY() ); + + for( int i = 0; i < NSEG; i++ ) + { + const Seg& currSeg = track.GetAt(i); +// Vec3d l2 = currSeg.pt - (currSeg.el - 0.5) * currSeg.norm; +// Vec3d r2 = currSeg.pt + (currSeg.er - 0.5) * currSeg.norm; +// Vec3d l2 = currSeg.pt - (currSeg.el + 0.5) * currSeg.norm; +// Vec3d r2 = currSeg.pt + (currSeg.er + 0.5) * currSeg.norm; +// Vec3d l2 = currSeg.pt - (currSeg.el + 0.0) * currSeg.norm; +// Vec3d r2 = currSeg.pt + (currSeg.er + 0.0) * currSeg.norm; + Vec3d l2 = currSeg.pt - (currSeg.el - margin) * currSeg.norm; + Vec3d r2 = currSeg.pt + (currSeg.er - margin) * currSeg.norm; + + Edge el(l1.x - _gridOrigin.x, l1.y - _gridOrigin.y, l2.x - _gridOrigin.x, l2.y - _gridOrigin.y); + if( el.sy <= el.ey && el.ey >= 0 && el.sy < GRID_SIZE ) + edges[max(0, el.sy)].push_back(el); + + Edge er(r1.x - _gridOrigin.x, r1.y - _gridOrigin.y, r2.x - _gridOrigin.x, r2.y - _gridOrigin.y); + if( er.sy <= er.ey && er.ey >= 0 && er.sy < GRID_SIZE ) + edges[max(0, er.sy)].push_back(er); + + l1 = l2; + r1 = r2; + + _leftPoints.push_back( lastSeg.pt.GetXY() - lastSeg.el * lastSeg.norm.GetXY() ); + _rightPoints.push_back( lastSeg.pt.GetXY() + lastSeg.er * lastSeg.norm.GetXY() ); + } + + // now have an edge list in bottom to top order. + for( int scan_y = 0; scan_y < GRID_SIZE; scan_y++ ) + { + vector& row = edges[scan_y]; + sort( row, scan_y ); + for( int i = 0; i + 1 < (int)row.size(); i += 2 ) + { + int x1 = MX(0, row[i].x); + int x2 = MN(GRID_SIZE - 1, row[i + 1].x ); + for( int x = x1; x <= x2; x++ ) + _grid[x][scan_y].removeEdgeMask(); + } + + if( scan_y + 1 < GRID_SIZE ) + { + vector& nextRow = edges[scan_y + 1]; + for( int i = 0; i < (int)row.size(); i++ ) + { + if( scan_y + 1 <= row[i].ey ) + nextRow.push_back(row[i]); + } + vector().swap(row); + } + } + + // make border cells unavailable. + for( int x = 0; x < GRID_SIZE; x++ ) + for( int y = 0; y < GRID_SIZE; y++ ) + if( x == 0 || y == 0 || x == GRID_SIZE - 1 || y == GRID_SIZE -1 ) + _grid[x][y].addEdgeMask(); +} + +bool Stuck::solve( const tCarElt* me ) +{ + LogSHADOW.debug( "[%d] stuck::solve (exp=%d, qlen=%d, best time=%g)\n", me->index, _expansionsN, _pqN.size(), _bestTime ); + + vector succs; + GridPoint car_pt1(*this, me, true, 0); + GridPoint car_pt2(car_pt1.x(), car_pt1.y(), car_pt1.iang(), false, 0, 0); + + for( int count = 0; count < 500 && !_pqN.empty(); ) + { + // pop position from edge priority list. + GridPoint pt = _pqN.top(); + _pqN.pop(); + + if( pt.est_time > _bestTime ) + continue; + + count++; + _expansionsN++; + + // generate list of successors. + generateSuccessorsN( pt, succs ); + + // for each successor. + for( vector::iterator it = succs.begin(); it != succs.end(); it++ ) + { + // if already visited with a better time --> continue. + Cell& succCell = at(*it); + if( succCell.times[it->fwang()] <= it->time ) + continue; + + // calc distance, and use to update time. + // fill in in visited. + succCell.times[it->fwang()] = it->time; + succCell.from[it->fwang()] = pt.pt; + + // got a new cell to search. + _pqN.push( *it ); + + // if car cell filled in, done. + if( car_pt1.pt == it->pt || car_pt2.pt == it->pt ) + { + _bestTime = it->time; + _bestPt = *it; + } + } + } + + if( !_pqN.empty() ) + { +// dumpGrid(); + return true; + } + + LogSHADOW.debug( "%d expansions\n", _expansionsN ); + LogSHADOW.debug( "best time: %g\n", _bestTime ); + LogSHADOW.debug( "best x: %d, y: %d, a: %d, fw %d\n", _bestPt.x(), _bestPt.y(), _bestPt.iang(), _bestPt.fw() ); + + if( fabs(_bestTime - 9e9f) < 1e8f ) + { + // failed to find a solution. + LogSHADOW.debug( "no solution!\n" ); + return false; + } + + _planIndex = 0; + _plan.clear(); + _plan.push_back( _bestPt ); + + int from = at(_bestPt).from[_bestPt.fwang()]; + float lastTime = 9e9f; + float time = at(_bestPt).times[_bestPt.fwang()]; + while( from >= 0 && time < lastTime ) + { + GridPoint pt(from); + LogSHADOW.debug( "from x: %d, y: %d, a: %d, fw %d, time %f\n", pt.x(), pt.y(), pt.iang(), pt.fw(), time ); + _plan.push_back( pt ); + + lastTime = time; + from = at(pt).from[pt.fwang()]; + time = at(pt).times[pt.fwang()]; + } + +// dumpGrid(); + + LogSHADOW.debug( "stuck::solve -- done\n" ); + + _stuckState = EXEC_PLAN; + _stuckTime = 0; + + return true; +} + +// search from car to destination. +bool Stuck::solveR( const tCarElt* me ) +{ + LogSHADOW.debug( "[%d] stuck::solveR (exp=%d, qlen=%d, best time=%g)\n", me->index, _expansionsR, _pqR.size(), _bestTime ); + + vector succs; + + for( int count = 0; count < 500 && !_pqR.empty(); ) + { + // pop position from edge priority list. + GridPoint pt = _pqR.top(); + _pqR.pop(); + + if( pt.est_time > _bestTime ) + continue; + + count++; + _expansionsR++; + + // generate list of successors. + generateSuccessorsR( pt, succs ); + + // for each successor. + Cell& fromCell = at(pt); + for( vector::iterator it = succs.begin(); it != succs.end(); it++ ) + { + // if already visited with a better time --> continue. + Cell& succCell = at(*it); + if( succCell.times[it->fwang()] <= it->time ) + continue; + + // calc distance, and use to update time. + // fill in in visited. + succCell.times[it->fwang()] = it->time; + succCell.from[it->fwang()] = pt.pt; + + // got a new cell to search. + _pqR.push( *it ); + + // if car cell filled in, done. + if( succCell.isSolution(it->fwang()) )//car_pt1.pt == it->pt || car_pt2.pt == it->pt ) + { + _bestTime = it->time; + _bestPt = *it; + } + } + } + + if( !_pqR.empty() ) + { +// dumpGrid(); + return true; + } + + LogSHADOW.debug( "%d expansions\n", _expansionsR ); + LogSHADOW.debug( "best time: %g\n", _bestTime ); + LogSHADOW.debug( "best x: %d, y: %d, a: %d, fw %d\n", _bestPt.x(), _bestPt.y(), _bestPt.iang(), _bestPt.fw() ); + + if( fabs(_bestTime - 9e9f) < 1e8f ) + { + // failed to find a solution. + LogSHADOW.debug( "no solution!\n" ); + return false; + } + +// dumpGrid(); + + _planIndex = 0; + _plan.clear(); + _plan.push_back( _bestPt ); + + int from = at(_bestPt).from[_bestPt.fwang()]; + float lastTime = 9e9f; + float time = at(_bestPt).times[_bestPt.fwang()]; + while( from >= 0 && time < lastTime ) + { + GridPoint pt(from); + LogSHADOW.debug( "from x: %d, y: %d, a: %d, fw %d, time %f\n", pt.x(), pt.y(), pt.iang(), pt.fw(), time ); + _plan.push_back( pt ); + + lastTime = time; + from = at(pt).from[pt.fwang()]; + time = at(pt).times[pt.fwang()]; + } + + std::reverse(_plan.begin(), _plan.end()); + for( int i = 0; i < (int)_plan.size() - 1; i++ ) + { + if( _plan[i].fw() != _plan[i + 1].fw() ) + _plan[i].set_fw( _plan[i + 1].fw() ); + } + + dumpGrid(); + + LogSHADOW.debug( "stuck::solveR -- done\n" ); + + _stuckState = EXEC_PLAN; + _stuckTime = 0; + + return true; +} + +void Stuck::dumpGrid() const +{ + set pts; + for( int i = 0; i < _plan.size(); i++ ) + { + const GridPoint& pt = _plan[i]; + pts.insert( pt.x() * 256 + pt.y() ); + } + + char line[GRID_SIZE + 1] = {0}; + for( int y = GRID_SIZE - 1; y >= 0; y-- ) + { + for( int x = 0; x < GRID_SIZE; x++ ) + { + const Cell& cell = at(x, y); + if( cell.isAvailable() ) + { + if( cell.est_time_to_car == 0 ) + line[x] = '@'; + else + { + if( pts.find(x * 256 + y) != pts.end() ) + line[x] = '*'; + else + { + int count = 0; + int ctimes = 0; + for( int i = 0; i < N_ANGLES * 2; i++ ) + { + if( cell.from[i] != -1 ) + count += 1; + if( cell.times[i] < 0 ) + ctimes += 1; + } + line[x] = count == N_ANGLES * 2 ? '~' : + count ? (count < 10 ? count + '0' : count - 10 + 'A') : + cell.est_time_to_dest < 0 ? '-' : + cell.est_time_to_dest == 0 ? '=' : + ctimes != 0 ? ':' : + cell.dist_from_walls < 4 ? '0' + cell.dist_from_walls : '.'; + } + } + } + else + line[x] = '#'; + } + + LogSHADOW.debug( "%s\n", line ); + } + + GridPoint pt(*this, _me, true, 0); + int iang = pt.iang(); + int octang = ((iang + 4) / 8) & 7; + int x = pt.x(); + int y = pt.y(); + int dx = delta8_x[octang]; + int dy = delta8_y[octang]; + float ft = at(x, y).times[fwang(iang, true)]; + float bt = at(x, y).times[fwang(iang, false)]; + LogSHADOW.debug( "[%2d,%2d] CAR iang %d ft %g bt %g\n", x, y, iang, ft, bt ); + + for( int i = 0; i < (int)_destinations.size(); i++ ) + { + const GridPoint& dest = _destinations[i]; + LogSHADOW.debug( "[%2d,%2d] DEST iang %d t %g\n", dest.x(), dest.y(), dest.iang(), at(dest).times[dest.fwang()] ); + } + + int xx = x - dx; + int yy = y - dy; + for( int da = -1; da <= 1; da++ ) + { + int fa = (iang + da) & 0x3F; + ft = at(xx, yy).times[fwang(fa, true)]; + bt = at(xx, yy).times[fwang(fa, false)]; + LogSHADOW.debug( "[%2d,%2d] iang %d ft %g bt %g\n", xx, yy, fa, ft, bt ); + } +} + +void Stuck::getUnstuck( const MyTrack& track, tCarElt* me, const tSituation* s ) +{ + LogSHADOW.debug( "[%d] stuck::getUnstuck\n", me->index ); + + if( _planIndex >= _plan.size() - 1 ) + { + _stuckState = RACING; + return; + } + + // look forward in the plan until either: + // 1. a change of forwards/backwards movement. + // 2. a substancial change in heading. + GridPoint car_pt1(*this, me, true, 0); + int best = -1; + double bestDist = 9e9; + LogSHADOW.debug( "[%d] (%d,%d) nearest pt: ", me->index, car_pt1.x(), car_pt1.y() ); + for( int i = _planIndex; i < _plan.size(); i++ ) + { + const GridPoint& pt = _plan[i]; + double dist = pt.dist(car_pt1); + LogSHADOW.debug( "[%d]=%g, ", i, dist ); + if( bestDist > dist ) + { + bestDist = dist; + best = i; + } + } + LogSHADOW.debug(" best=%d\n", best ); + + if( best < 0 ) + { + // failed to find a best point to follow. + _stuckState = REINIT; + _stuckTime = 0.0; + return; + } + + int ahead = best + 1 < _plan.size() ? best + 1 : best; + double heading = _plan[ahead].iang() * 2 * PI / 64; + double deltaAng = heading - me->pub.DynGC.pos.az; + NORM_PI_PI(deltaAng); + + if( bestDist > 1.0 || deltaAng > 20 * PI / 180 ) + { + // too far from path. start again. + _stuckState = REINIT; + _stuckTime = 0.0; + return; + } + + _planIndex = best; + bool fw = _plan[best].fw(); + float spd = me->pub.DynGC.vel.x; + int gear = fw ? 1 : -1; +// me->ctrl.accelCmd = MN(0.25f, (8 - fabs(spd)) * 0.05); +// me->ctrl.accelCmd = MN(0.25f, (10 - fabs(spd)) * 0.25); +// me->ctrl.accelCmd = MN(0.50f, (10 - fabs(spd)) * 0.25); + me->ctrl.accelCmd = MN(0.75f, (10 - fabs(spd)) * 0.25); + me->ctrl.brakeCmd = fw && spd < -0.1 || !fw && spd > 0.1 ? 0.5 : 0; + me->ctrl.clutchCmd = 0;//gear == me->ctrl.gear ? 0.0f : 0.5f; + me->ctrl.gear = gear; + me->ctrl.steer = (spd > 0 ? deltaAng : -deltaAng) * 2 / me->info.steerLock; + + double dist = calcCarDist(fw, 10, me, s); + LogSHADOW.debug( "[%d] dir=%d dist=%g\n", me->index, fw, dist ); + if( dist < 0.2 ) + { + me->ctrl.accelCmd = 0; + //me->ctrl.brakeCmd = 1; + me->ctrl.gear = -me->ctrl.gear; + _stuckTime += s->deltaTime; + + if( _stuckTime > 1 ) + { + _stuckState = REINIT; + _stuckTime = 0.0; + return; + } + } + + LogSHADOW.info( "[%d] plan index: %d/%d acc=%.3f, gear=%d, da=%.3f, steer=%.3f, dist-ahead=%.3f\n", + me->index, _planIndex, _plan.size(), me->ctrl.accelCmd, me->ctrl.gear, deltaAng * 180 / PI, + me->ctrl.steer * me->info.steerLock * 180 / PI, dist ); +} + +double Stuck::calcCarDist( bool fw, double maxDist, const tCarElt* me, const tSituation* s ) const +{ + double dist = maxDist; + CarBounds2d bounds(me); + for( int i = 0; i < s->raceInfo.ncars; i++ ) + { + const tCarElt* oCar = s->cars[i]; + if( oCar == me || (oCar->pub.state & RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT) != 0 ) + continue; + + const CarBounds2d oBounds(oCar); + + dist = bounds.distToSide(fw ? CarBounds2d::SIDE_FRONT : CarBounds2d::SIDE_REAR, dist, oBounds); + } + + dist = bounds.distToSide(fw ? CarBounds2d::SIDE_FRONT : CarBounds2d::SIDE_REAR, dist, _leftPoints); + dist = bounds.distToSide(fw ? CarBounds2d::SIDE_FRONT : CarBounds2d::SIDE_REAR, dist, _rightPoints); + + return dist; +} + +void Stuck::generateSuccessorsN( const GridPoint& from, vector& succs ) const +{ + succs.clear(); + + int x = from.x(); + int y = from.y(); + int a = from.iang(); + + for( int da = -1; da <= 1; da++ ) + { + int af = (a + da) & ANGLE_MASK; + int oct_ang = ((af + HALF_OCTANT) / OCTANT) & 7; + + int dx = delta8_x[oct_ang]; + int dy = delta8_y[oct_ang]; +// float dt = delta8_t[oct_ang]; + float dt = delta64_t[af]; + + // forwards direction. + const Cell& fSuccCell1 = at(x - dx, y - dy); +/* if( fSuccCell1.isAvailable() ) + { + const Cell& fSuccCell2 = at(x - 2 * dx, y - 2 * dy); + if( fSuccCell2.isAvailable() ) + { + float fSuccTime = from.time + dt + from.bw() * 1.5f; + float fSuccEst = fSuccTime + fSuccCell1.est_time_to_car; + succs.push_back( GridPoint(x - dx, y - dy, af, true, fSuccTime, fSuccEst) ); + } + }*/ + if( fSuccCell1.isAvailable(fwang(af, true)) ) + { + float fSuccTime = from.time + dt + from.bw() * 1.5f; + float fSuccEst = fSuccTime + fSuccCell1.est_time_to_car; + succs.push_back( GridPoint(x - dx, y - dy, af, true, fSuccTime, fSuccEst) ); + } + + // backwards direction. + const Cell& bSuccCell1 = at(x + dx, y + dy); +/* if( bSuccCell1.isAvailable() ) + { + const Cell& bSuccCell2 = at(x + 2 * dx, y + 2 * dy); + if( bSuccCell2.isAvailable() ) + { + float bSuccTime = from.time + dt + from.fw() * 1.5f; + float bSuccEst = bSuccTime + bSuccCell1.est_time_to_car; + succs.push_back( GridPoint(x + dx, y + dy, af, false, bSuccTime, bSuccEst) ); + } + }*/ + if( bSuccCell1.isAvailable(fwang(af, false)) ) + { + float bSuccTime = from.time + dt + from.fw() * 1.5f; + float bSuccEst = bSuccTime + bSuccCell1.est_time_to_car; + succs.push_back( GridPoint(x + dx, y + dy, af, false, bSuccTime, bSuccEst) ); + } + } +} + +void Stuck::generateSuccessorsR( const GridPoint& from, vector& succs ) const +{ + succs.clear(); + + int x = from.x(); + int y = from.y(); + int a = from.iang(); + + for( int da = -1; da <= 1; da++ ) + { + int af = (a + da) & ANGLE_MASK; +// int oct_ang = ((af + HALF_OCTANT) / OCTANT) & 7; + int oct_ang = ((a + HALF_OCTANT) / OCTANT) & 7; + + int dx = delta8_x[oct_ang]; + int dy = delta8_y[oct_ang]; +// float dt = delta8_t[oct_ang]; + float dt = delta64_t[af]; + + // forwards direction. + const Cell& fSuccCell1 = at(x + dx, y + dy); + if( fSuccCell1.isAvailable() ) + { + const Cell& fSuccCell2 = at(x + 2 * dx, y + 2 * dy); + if( fSuccCell2.isAvailable() ) + { + float fSuccTime = from.time + dt + from.bw() * 1.5f + (fSuccCell2.dist_from_walls != 1 ? 0 : 1); + float fSuccEst = fSuccTime + fSuccCell1.est_time_to_dest; + succs.push_back( GridPoint(x + dx, y + dy, af, true, fSuccTime, fSuccEst) ); + } + } + + // backwards direction. + const Cell& bSuccCell1 = at(x - dx, y - dy); + if( bSuccCell1.isAvailable() ) + { + const Cell& bSuccCell2 = at(x - 2 * dx, y - 2 * dy); + if( bSuccCell2.isAvailable() ) + { + float bSuccTime = from.time + dt + from.fw() * 1.5f + (bSuccCell2.dist_from_walls != 1 ? 0 : 1);; + float bSuccEst = bSuccTime + bSuccCell1.est_time_to_dest; + succs.push_back( GridPoint(x - dx, y - dy, af, false, bSuccTime, bSuccEst) ); + } + } + } +} diff --git a/src/drivers/shadow/src/Stuck.h b/src/drivers/shadow/src/Stuck.h new file mode 100644 index 000000000..ebe1ddcab --- /dev/null +++ b/src/drivers/shadow/src/Stuck.h @@ -0,0 +1,319 @@ +#ifndef _STUCK_H +#define _STUCK_H + +#include +#include + +#include +#include + +#include "Utils.h" +#include "MyTrack.h" +#include "Opponent.h" + +// The "SHADOW" logger instance. +extern GfLogger* PLogSHADOW; +#define LogSHADOW (*PLogSHADOW) + +class Stuck +{ +public: + Stuck(); + ~Stuck(); + + bool execute( const MyTrack& track, const tSituation* s, tCarElt* me, const Opponent::Sit& mySit ); + +private: + void executeRacing( const MyTrack& track, const tSituation* s, const tCarElt* me, const Opponent::Sit& mySit ); + void executeReorient(const MyTrack& track, const tSituation* s, tCarElt* me, const Opponent::Sit& mySit ); + void executeInit( const MyTrack& track, const tSituation* s, tCarElt* me ); + void executeSolving( const MyTrack& track, const tSituation* s, tCarElt* me ); + void executePlan( const MyTrack& track, const tSituation* s, tCarElt* me ); + +private: + enum { N_ANGLES = 64 }; + enum { HALF_ANGLE = N_ANGLES / 2 }; + enum { ANGLE_MASK = N_ANGLES - 1 }; + enum { OCTANT = N_ANGLES / 8 }; + enum { HALF_OCTANT = OCTANT / 2 }; + + enum { GRID_RAD = 50 }; + enum { GRID_SIZE = 2 * GRID_RAD + 1 }; + enum { CAR_RAD = 4 }; + + enum { SPD = 4 }; // speed of car while getting unstuck. + + enum State { RACING, REORIENT_FORWARDS, REORIENT_BACKWARDS, REINIT, SOLVING, EXEC_PLAN }; + + static inline int fwang( int iang, bool fw ) { return (iang << 1) | int(fw); } + + struct GridPoint + { + unsigned int pt; // current position. + float est_time; // estimated time to finish. + float time; // current time. + + GridPoint() + : pt(0), est_time(0), time(0) + { + } + + GridPoint( int p ) + : pt(p), est_time(0), time(0) + { + } + + GridPoint( int x, int y, int iang, bool fw, float time, float est ) + { + set(x, y, iang, fw, time, est); + } + + GridPoint( int x, int y, float ang, bool fw, float time, float est ) + { + set(x, y, ang, fw, time, est); + } + + GridPoint( const Stuck& stuck, const tCarElt* car, bool fw, float est ) + { + float dx = car->pub.DynGCg.pos.x - stuck._gridOrigin.x; + float dy = car->pub.DynGCg.pos.y - stuck._gridOrigin.y; + int x = (int)floor(dx + 0.5); + int y = (int)floor(dy + 0.5); + set(x, y, (float)car->pub.DynGCg.pos.az, fw, 0.0f, est); + } + + bool operator<( const GridPoint& other ) const + { + return est_time > other.est_time; + } + + bool isValid() const + { + return false; + } + + void set( int x, int y, int iang, bool fw, float tm, float est ) + { + x = x & 0xFF; + y = y & 0xFF; + iang = iang & ANGLE_MASK; + pt = (fw << 24) | (x << 16) | (y << 8) | (iang); + time = tm; + est_time = est; + } + + void set( int x, int y, float ang, bool fw, float time, float est ) + { + int iang = to_iang(ang); + set(x, y, iang, fw, time, est); + } + + void set_fw( bool fw ) + { + pt = (fw << 24) | (0x00FFFFFF & pt); + } + + double dist( const GridPoint& other ) const + { + int dx = x() - other.x(); + int dy = y() - other.y(); + int da = iang() - other.iang(); + if( da > 32 ) + da -= 64; + else if( da < -32 ) + da += 64; + + return dx*dx + dy*dy + da*da*0.001; + } + + bool fw() const { return (pt >> 24) != 0; } + bool bw() const { return (pt >> 24) == 0; } + int x() const { return (pt >> 16) & 0xFF; } + int y() const { return ((pt >> 8) & 0xFF); } + int iang() const { return pt & 0xFF; } + int fwang() const { return (iang() << 1) | int(fw()); } + }; + + struct Cell + { + enum { EDGE_MASK = 0x80000000 }; + unsigned int occupied_mask; + float est_time_to_car; + float est_time_to_dest; + int dist_from_walls; + float times[N_ANGLES * 2]; // indexed by fwang + int from[N_ANGLES * 2]; // indexed by fwang + char solution[N_ANGLES * 2]; // indexed by fwang + + + Cell() { clear(); } + + void clear() + { + occupied_mask = EDGE_MASK; // track edges... + //occupied_mask = 0; + est_time_to_car = -1; + est_time_to_dest = -1; + dist_from_walls = 0; + for( int i = 0; i < N_ANGLES * 2; i++ ) + { + times[i] = 9e9f; + from[i] = -1; + solution[i] = 0; + } + } + + void addCarMask( int carIdx ) { occupied_mask |= (1u << carIdx); }; + void removeCarMask( int carIdx ) { occupied_mask &= ~(1u << carIdx); }; + void clearAllCarMasks() { occupied_mask &= EDGE_MASK; }; + + void addEdgeMask() { occupied_mask |= EDGE_MASK; }; + void removeEdgeMask() { occupied_mask &= ~EDGE_MASK; }; + + bool isAvailable() const { return occupied_mask == 0; } + bool isAvailable( int fwang ) const { return occupied_mask == 0 && times[fwang] >= 0; } + + void setSolution( int fwang ) { solution[fwang] = 1; times[fwang] = 9e9f; } + bool isSolution( int fwang ) const { return solution[fwang] == 1; } + }; + + struct Edge + { + int sy; // starting y value + int ey; // ending y value + float sx; // starting x value + float dX; // for a step of size 1 in y + int x; // current x value + + Edge( float x1, float y1, float x2, float y2 ) + { + if( y1 > y2 ) + { + std::swap( x1, x2 ); + std::swap( y1, y2 ); + } + + sy = (int)ceil(y1); + ey = (int)floor(y2); + dX = y1 < y2 ? (x2 - x1) / (y2 - y1) : 0; + sx = x1 + (sy - y1) * dX; + x = 0; + } + + bool operator<( const Edge& other ) const + { + return x < other.x; + } + + int calcX(int y) { return (int)floor(sx + (y - sy) * dX); } + }; + + struct OppInfo + { + double x; + double y; + int ix; + int iy; + const tCarElt* car; + + OppInfo() : x(0), y(0), ix(0), iy(0), car(0) {} + OppInfo( double X, double Y, const tCarElt* CAR ) + : x(X), y(Y), car(CAR) + { + ix = (int)floor(x + 0.5); + iy = (int)floor(x + 0.5); + } + + bool operator==( const OppInfo& other ) const + { + return ix == other.ix && iy == other.iy && car == other.car; + } + + bool operator!=( const OppInfo& other ) const + { + return !operator==(other); + } + }; + + static const int delta8_x[8]; + static const int delta8_y[8]; + static const float delta8_t[8]; + static const float delta64_t[64]; + +private: + void updateStuckTime( const tCarElt* me, const tSituation* s ); + void makeOpponentsList( const tSituation* s, const tCarElt* me, + std::vector* opponents ); + bool clearAhead( const MyTrack& track, const tSituation* s, const tCarElt* me ) const; + bool opponentsChanged( const tSituation* s, const tCarElt* me ); + + bool isInitialised() const { return _me != 0; } + void reorient( const tCarElt* me, double dirAng ); + void init( const MyTrack& track, const tSituation* s, const tCarElt* me ); + void fillCarCells( int carI, double carX, double carY, double carAng, double dx, double dy, double rad, bool addMask ); + void fillTrackCells( const MyTrack& track ); + bool solve( const tCarElt* me ); + bool solveR( const tCarElt* me ); + void dumpGrid() const; + void getUnstuck( const MyTrack& track, tCarElt* me, const tSituation* s ); + +private: + const Cell& at( int x, int y ) const { return _grid[x][y]; } + Cell& at( int x, int y ) { return _grid[x][y]; } + + const Cell& at( const GridPoint& pt ) const { return _grid[pt.x()][pt.y()]; } + Cell& at( const GridPoint& pt ) { return _grid[pt.x()][pt.y()]; } + + bool isValid( int x, int y ) const { return x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE; } + + const Cell& operator[]( const GridPoint& pt ) const { return at(pt); } + + double calcCarDist( bool fw, double maxDist, const tCarElt* me, const tSituation* s ) const; + + void generateSuccessorsN( const GridPoint& from, std::vector& succs ) const; + void generateSuccessorsR( const GridPoint& from, std::vector& succs ) const; + + static int to_iang( double ang ) + { + const float ang_step = float(N_ANGLES / (2 * 3.14159265)); + int iang = (int)floor(ang * ang_step + 0.5); + return iang & ANGLE_MASK; + } + + static int reverse( int iang ) { return (iang + N_ANGLES / 2) & ANGLE_MASK; } + + static void sort( std::vector& row, int y ); + +private: + const tCarElt* _me; + Vec2d _gridOrigin; + std::vector> _grid; + State _stuckState; + double _stuckTime; + int _stuckCount; + + // track points for collision detection. + std::vector _leftPoints; + std::vector _rightPoints; + + // intialisation variables + std::vector _opponents; + GridPoint _origCarPt; + + // for debugging + std::vector _destinations; + + // solver variables + int _expansionsN; + std::priority_queue _pqN; + int _expansionsR; + std::priority_queue _pqR; + float _bestTime; + GridPoint _bestPt; + + // plan variables + std::vector _plan; + int _planIndex; +}; + +#endif // _STUCK_H