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); }