Point3D CalcTrack(
const Point2D& towerPosition,
double timeStep,
const PointSet& altitudeTrack,
double fixRange,
double fixBearing,
double elapsedTime,
double heading,
double initialBankRate,
double bankRateAccel,
double windHeading,
const PointSet& windSpeeds,
const PointSet& planeSpeeds,
Track3D* track
)
{
if(track != NULL)
{
track->clear();
}
// Set up the initial conditions.
double x = towerPosition.x_ + fixRange * sin(fixBearing);
double y = towerPosition.y_ + fixRange * cos(fixBearing);
double z = 0.0;
double time = 0.0;
double bankRate = initialBankRate;
windHeading = (M_PI / -2) - windHeading; // reversed as wind direction is where the wind is FROM
double sinWind = sin(windHeading);
double cosWind = cos(windHeading);
while(time < elapsedTime)
{
// Get the time for this point.
double thisStep = timeStep;
double newTime = time + timeStep;
if(newTime > elapsedTime)
{
// Last step is incomplete.
thisStep = elapsedTime - time;
newTime = elapsedTime;
}
time = newTime;
// Interpolate the plane characteristics at this time.
double altitude = altitudeTrack.interpolate(time);
double windSpeed = windSpeeds.interpolate(altitude);
double planeSpeed = planeSpeeds.interpolate(time);
// Update the simulated plane location.
heading += bankRate * thisStep;
bankRate += bankRateAccel * thisStep;
double hdg = (M_PI / 2) - heading;
z = altitude;
x += thisStep * (planeSpeed * cos(hdg) + windSpeed * cosWind);
y += thisStep * (planeSpeed * sin(hdg) + windSpeed * sinWind);
if(track != NULL)
{
track->addPoint(x, y, z);
}
}
return Point3D(x, y, z);
}
