gcblue-commits

[Dewitt C. <ddc...@us...>]

Update of /cvsroot/gcblue/gcb_wx/src/ai
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11821/src/ai

Added Files:
	Nav.cpp 
Log Message:
Better sensor ageout behavior, more ai work, misc cleanup

--- NEW FILE: Nav.cpp ---
/**
**  @file Nav.cpp
*/
/*  Copyright (C) 2005 Dewitt Colclough (de...@tw...)
**  All rights reserved.

**  This file is part of the Global Conflict Blue (GCB) program.
**  GCB is free software; you can redistribute it and/or modify
**  it under the terms of version 2 of the GNU General Public License as 
**  published by the Free Software Foundation.

**  GCB is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.

**  You should have received a copy of the GNU General Public License
**  along with GCB; if not, write to the Free Software
**  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "stdwx.h"

#ifndef WX_PRECOMP
#include "wx/wx.h" 
#endif

#include "ai/Nav.h"
#include "scriptinterface/tcPlatformInterface.h"
#include "simmath.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif 

using namespace ai;
using ScriptInterface::tcPlatformInterface;

void Nav::AddWaypoint(double lon_rad, double lat_rad, float alt_m)
{
    GeoPoint waypoint;
    waypoint.Set(lon_rad, lat_rad, alt_m);

    waypoints.push_back(waypoint);
}

void Nav::ClearWaypoints()
{
    waypoints.clear();
}

/**
* @return waypoint vector of GeoPoint
*/
const std::vector<GeoPoint>& Nav::GetWaypoints() const
{
    return waypoints;
}

void Nav::Update(double t)
{
    if (!IsReadyForUpdate(t)) return;

    if (currentWaypoint >= waypoints.size())
    {
        EndTask();
        return;
    }

    // return if conn is locked by another higher priority task
    if (!Write("ConnLock", "")) return; 

    tcPlatformInterface platformInterface = GetPlatformInterface();

    GeoPoint& waypoint = waypoints[currentWaypoint];
    double lon_rad = waypoint.mfLon_rad;
    double lat_rad = waypoint.mfLat_rad;
    float alt_m = waypoint.mfAlt_m;
    
    float heading_deg = platformInterface.GetHeadingToDatum(lon_rad, lat_rad);
    float range_m = 1000.0f * platformInterface.GetRangeToDatum(lon_rad, lat_rad);
    float speed_mps = C_KTSTOMPS * platformInterface.GetSpeed();
    float eta_s = (speed_mps > 0) ? (range_m / speed_mps) : 9999.0f;

    if (eta_s >= 30.0f)
    {
        SetUpdateInterval(20.0f);
    }
    else if (eta_s >= 2.0f)
    {
        SetUpdateInterval(1.0f);
    }
    else
    {
        SetUpdateInterval(1.0f);
        currentWaypoint++;
    }

    platformInterface.SetHeading(heading_deg);

    FinishUpdate(t);
}

Nav::Nav(tcPlatformObject* platform_, Blackboard* bb, 
                                   long id_, double priority_, const std::string& taskName_)
:   Task(platform_, bb, id_, priority_, taskName_),
    currentWaypoint(0)
{
    wxASSERT(platform);
}



Nav::~Nav()
{
}