Mission_handler_navigating::Mission_handler_navigating( const INS& ins,
const Mavlink_stream& mavlink_stream,
Mavlink_waypoint_handler& waypoint_handler):
Mission_handler(),
ins_(ins),
mavlink_stream_(mavlink_stream),
waypoint_handler_(waypoint_handler),
waypoint_reached_(false),
start_time_(0),
travel_time_(0)
{
waypoint_ = Waypoint( MAV_FRAME_LOCAL_NED,
MAV_CMD_NAV_WAYPOINT,
0,
0.0f,
0.0f,
0.0f,
0.0f,
0.0f,
0.0f,
0.0f);
position_command_ = position_command_t{{{0.0f, 0.0f, 0.0f}},
0.0f};
}
vector<Waypoint> AI::findPath(Waypoint start, Waypoint goal, int enemyType)
{
int startN, goalN;
vector<Waypoint> wayPoints;
startN = (start.x/quadSize) + ((start.y/quadSize) * mapSize);
goalN = goal.x + (goal.y*mapSize);
if(saved[startN][goalN])
{
return *saved[startN][goalN]; //Returnar en saved path
}
lua_getglobal(pathScript, "astar");
lua_pushnumber(pathScript, startN);
lua_pushnumber(pathScript, goalN);
lua_pushnumber(pathScript, enemyType);
lua_pcall(pathScript, 3, 2, 0); //kalla på funktionen
//hämta värden
int a = (int)lua_tonumber(pathScript, -1);
lua_pop(pathScript, 1);
wayPoints.reserve(a);
if(a > 0)
{
//Hämta ut tabellen
lua_pushnil(pathScript);
while(lua_next(pathScript, -2) != 0)
{
int temp = (int)lua_tonumber(pathScript, -1);
wayPoints.push_back(Waypoint(((int)temp % mapSize) * quadSize, ((int)temp / mapSize) * quadSize));
lua_pop(pathScript, 1);
}
lua_pop(pathScript, 1);
}
saved[startN][goalN] = new vector<Waypoint>();
saved[startN][goalN]->reserve(wayPoints.size());
for(int i = 0; i < (int)wayPoints.size(); i++)
{
saved[startN][goalN]->push_back(wayPoints.at(i)); //Sparar pathen
}
if(goalN != startN)
{
saved[goalN][startN] = new vector<Waypoint>();
saved[goalN][startN]->reserve(wayPoints.size());
for(int i = wayPoints.size()-1; i >= 0; i--)
{
saved[goalN][startN]->push_back(wayPoints.at(i)); //Sparar pathen åt andra hållet
}
}
return wayPoints;
}
Waypoint LoadData::getWayPointbyID(string id) {
for (int unsigned i = 0; i < waypoints.size(); i++)
if (waypoints[i].getID() == id)
return waypoints[i];
for (int unsigned i = 0; i < airports.size(); i++)
if (airports[i].getID() == id)
return airports[i];
return Waypoint();
}
const Waypoint &
Waypoints::CheckExistsOrAppend(const Waypoint &waypoint)
{
const Waypoint* found = LookupName(waypoint.name);
if (found && found->IsCloseTo(waypoint.location, fixed(100))) {
return *found;
}
return Append(Waypoint(waypoint));
}
static Waypoint
MakeWaypoint(double longitude, double latitude, double altitude)
{
return MakeWaypoint(Waypoint(MakeGeoPoint(longitude, latitude)), altitude);
}
ePathFinderStatus cPathFinder::GetNextWayPoint(cChunk & a_Chunk, const Vector3d & a_Source, Vector3d * a_Destination, Vector3d * a_OutputWaypoint, bool a_DontCare)
{
m_FinalDestination = *a_Destination;
m_Source = a_Source;
// If a recent PATH_NOT_FOUND was returned, we rest for a few ticks.
if (m_NotFoundCooldown > 0)
{
m_NotFoundCooldown -= 1;
return ePathFinderStatus::CALCULATING;
}
// Tweak the destination. If something is wrong with the destination or the chunk, rest for a while.
if (!(EnsureProperPoint(m_FinalDestination, a_Chunk) && EnsureProperPoint(m_Source, a_Chunk)))
{
m_NotFoundCooldown = 20;
return ePathFinderStatus::PATH_NOT_FOUND;
}
/* printf("%d %d %d -> %d %d %d\n",
static_cast<int>(m_Source.x),
static_cast<int>(m_Source.y),
static_cast<int>(m_Source.z),
static_cast<int>(m_FinalDestination.x),
static_cast<int>(m_FinalDestination.y),
static_cast<int>(m_FinalDestination.z)); */
// Rest is over. Prepare m_Path by calling ResetPathFinding.
if (m_NotFoundCooldown == 0)
{
m_NotFoundCooldown = -1;
ResetPathFinding(a_Chunk);
}
// If m_Path has not been initialized yet, initialize it.
if (!m_Path->IsValid())
{
ResetPathFinding(a_Chunk);
}
switch (m_Path->CalculationStep(a_Chunk))
{
case ePathFinderStatus::NEARBY_FOUND:
{
m_NoPathToTarget = true;
m_PathDestination = m_Path->AcceptNearbyPath();
if (a_DontCare)
{
m_FinalDestination = m_PathDestination;
*a_Destination = m_FinalDestination; // Modify the mob's final destination because it doesn't care about reaching an exact spot
}
else
{
m_DeviationOrigin = m_FinalDestination; // This is the only case in which m_DeviationOrigin != m_PathDestination
}
return ePathFinderStatus::CALCULATING;
// The next call will trigger the PATH_FOUND case
}
case ePathFinderStatus::PATH_NOT_FOUND:
{
m_NotFoundCooldown = 20;
return ePathFinderStatus::PATH_NOT_FOUND;
}
case ePathFinderStatus::CALCULATING:
{
return ePathFinderStatus::CALCULATING;
}
case ePathFinderStatus::PATH_FOUND:
{
m_GiveUpCounter -= 1;
if (m_GiveUpCounter == 0)
{
if (a_DontCare)
{
// We're having trouble reaching the next waypoint but the mob
// Doesn't care where to go, just tell him we got there ;)
m_FinalDestination = m_Source;
*a_Destination = m_FinalDestination;
ResetPathFinding(a_Chunk);
return ePathFinderStatus::CALCULATING;
}
else
{
ResetPathFinding(a_Chunk);
return ePathFinderStatus::CALCULATING;
}
}
if (PathIsTooOld())
{
ResetPathFinding(a_Chunk);
return ePathFinderStatus::CALCULATING;
}
if (m_Path->NoMoreWayPoints())
{
// We're always heading towards m_PathDestination.
// If m_PathDestination is exactly m_FinalDestination, then we're about to reach the destination.
if (m_PathDestination == m_FinalDestination)
{
*a_OutputWaypoint = m_FinalDestination;
return ePathFinderStatus::PATH_FOUND;
}
else
{
// Otherwise, we've finished our approximate path and time to recalc.
ResetPathFinding(a_Chunk);
return ePathFinderStatus::CALCULATING;
}
}
Vector3d Waypoint(m_WayPoint);
Vector3d Source(m_Source);
Waypoint.y = 0;
Source.y = 0;
if (m_Path->IsFirstPoint() || (((Waypoint - Source).SqrLength() < WAYPOINT_RADIUS) && (m_Source.y >= m_WayPoint.y)))
{
// if the mob has just started or if the mob reached a waypoint, give them a new waypoint.
m_WayPoint = m_Path->GetNextPoint();
m_GiveUpCounter = 40;
return ePathFinderStatus::PATH_FOUND;
}
else
{
// Otherwise, the mob is still walking towards its waypoint, we'll patiently wait. We won't update m_WayPoint.
*a_OutputWaypoint = m_WayPoint;
return ePathFinderStatus::PATH_FOUND;
}
}
#ifndef __clang__
default:
{
return ePathFinderStatus::PATH_FOUND;
// Fixes GCC warning: "control reaches end of non-void function".
}
#endif
}
}
