OSG 根据经纬度、方向角和距离计算下一个点的经纬度

飞机自由飞行中需要根据参数计算出下一个点的坐标： 

osg::Vec3d calLonLat(double lon, double lat, double height, double yaw,  double distance)
{
    double R = 6378137; // 球半径
    double sinLat = sin(osg::DegreesToRadians(lat));
    double cosLat = cos(osg::DegreesToRadians(lat));
    double cosDistR = cos(distance / R);
    double sinDistR = sin(distance / R);
    double lat2 = asin(sinLat * cosDistR + cosLat * sinDistR * cos(osg::DegreesToRadians(yaw)));
    double lon2 = osg::DegreesToRadians(lon) + atan2(sin(osg::DegreesToRadians(yaw)) * sinDistR * cosLat, cosDistR - sinLat * sin(lat2) );

    lon2 = osg::RadiansToDegrees(lon2);
    lon2 = lon2 > 180 ? lon2 - 360 : lon2 < -180 ? lon2 + 360 : lon2;
    lat2 = osg::RadiansToDegrees(lat2);

    return osg::Vec3d(lon2,lat2, height);
}