飞机沿设置路径飞行以及跟踪飞行物（十一）

 一、路径生成

void COSGObject::DoAPreLine()
{
    osg::ref_ptr<osg::Vec4Array> vaTemp = new osg::Vec4Array;
    vaTemp->push_back(osg::Vec4(109.1347, 34.3834, 500, 10));
    vaTemp->push_back(osg::Vec4(109.1174, 34.3686, 600, 50));
    vaTemp->push_back(osg::Vec4(108.8794, 34.1944, 3000, 200));
    vaTemp->push_back(osg::Vec4(107.1302, 34.3941, 10000, 200));
    vaTemp->push_back(osg::Vec4(108.9387, 34.9202, 30000, 200));
    vaTemp->push_back(osg::Vec4(109.5066, 34.51, 1000, 200));
    vaTemp->push_back(osg::Vec4(109.1347, 34.3834, 537, 0));
    apc = CreateAirLinePath(vaTemp);
}

//根据输入的控制点，输出一个路径，控制点格式为（经，纬，高，速）
osg::AnimationPath* COSGObject::CreateAirLinePath(osg::Vec4Array* ctrl)
{
    osg::ref_ptr<osg::AnimationPath> animationPath = new osg::AnimationPath;
    animationPath->setLoopMode(osg::AnimationPath::NO_LOOPING);

    double shuiPingAngle;
    double chuiZhiAngle;
    double time = 0;

    osg::Matrix matrix;
    osg::Quat _rotation;

    //当前点
    osg::Vec3d positionCur;
    //下一点
    osg::Vec3d positionNext;

    for(osg::Vec4Array::iterator iter = ctrl->begin(); iter != ctrl->end(); iter++)
    {
        osg::Vec4Array::iterator iter2 = iter;
        iter2 ++;

        //需要判断是不是已经到顶
        //iter2++
        if(iter2 == ctrl->end())
        {
            break;
        }

        double x, y, z;
        csn->getEllipsoidModel()->convertLatLongHeightToXYZ(osg::DegreesToRadians(iter->y()), osg::DegreesToRadians(iter->x()), iter->z(), x, y, z);
        positionCur = osg::Vec3(x, y, z);
        csn->getEllipsoidModel()->convertLatLongHeightToXYZ(osg::DegreesToRadians(iter2->y()), osg::DegreesToRadians(iter2->x()), iter2->z(), x, y, z);
        positionNext = osg::Vec3(x, y, z);

        //求出水平夹角
        if(iter->x() == iter2->x())
        {
            shuiPingAngle = osg::PI_2;            
        }
        else
        {
            shuiPingAngle = atan((iter2->y() - iter->y())/(iter2->x() - iter->x()));
            if(iter2->x() > iter->x())
            {
                shuiPingAngle += osg::PI;
            }
        }

        //求垂直夹角
        if(iter->z() == iter2->z())
        {
            chuiZhiAngle = 0;
        }
        else
        {
            if(0 == sqrt(pow(GetDis(positionCur, positionNext), 2)) - pow((iter2->z() - iter->z()), 2))
            {
                chuiZhiAngle = osg::PI_2;
            }
            else
            {
                chuiZhiAngle = atan(  (iter2->z() - iter->z()) /  sqrt(pow(GetDis(positionCur, positionNext), 2)) - pow((iter2->z() - iter->z()), 2) );
            }

            if(chuiZhiAngle>= osg::PI_2)
                chuiZhiAngle = osg::PI_2;
            if(chuiZhiAngle<= -osg::PI_2)
            {
                chuiZhiAngle = -osg::PI_2;
            }
        }

        //求飞机的变换矩阵
        csn->getEllipsoidModel()->computeLocalToWorldTransformFromLatLongHeight(osg::DegreesToRadians(iter->y()), osg::DegreesToRadians(iter->x()), iter->z(), matrix);
        _rotation.makeRotate(0, osg::Vec3(1.0, 0.0, 0.0), chuiZhiAngle+osg::PI_2, osg::Vec3(0.0, 1.0, 0.0), shuiPingAngle, osg::Vec3(0.0, 0.0, 1.0));
        matrix.preMultRotate(_rotation);
        animationPath->insert(time, osg::AnimationPath::ControlPoint(positionCur, matrix.getRotate()));
        
        //把下一个点的时间求出来
        time += GetRunTime(positionCur, positionNext, iter2->w());
    }

    animationPath->insert(time, osg::AnimationPath::ControlPoint(positionNext, matrix.getRotate()));
    return animationPath.release();
}

double COSGObject::GetDis(osg::Vec3 from, osg::Vec3 to)
{
    return sqrt( (to.x() - from.x())*(to.x() - from.x()) +  (to.y() - from.y())*(to.y() - from.y()) + (to.z() - from.z())*(to.z() - from.z()));
}

double  COSGObject::GetRunTime(osg::Vec3 from, osg::Vec3 to, double speed)
{
    double dist = GetDis(from, to);
    if(speed == 0)
        return 1000000000;
    return dist/speed;
}

二、 跟踪飞行物

//获取当前操作器的视点，将模型放进这个视角中，然后设置这个视角的一些参数，比如从哪个角度和距离观察模型。然后将这个视点设置为操作器的视点。

void COSGObject::DoPreLineNow()
{
    theApp.bNeedModify = TRUE;
    while(!theApp.bCanModify)Sleep(1);

    mtFlySelf->setMatrix(osg::Matrixd::scale(10, 10, 10)* osg::Matrixd::rotate(1.57, osg::Vec3(0, 0, 1)));
    mtfly->setUpdateCallback(new osg::AnimationPathCallback(apc, 0.0, 1.0));
    em->setViewpoint(osgEarth::Viewpoint("",109.1347, 34.3834, 437, 24.261, -21.6, 1000), 1);
    //em->setNode(flyAirport);

	osgEarth::Viewpoint vp = em->getViewpoint();
	vp.setNode(flyAirport);
	vp.range()->set(250.0, osgEarth::Units::METERS);//观察的距离
	vp.pitch()->set(-45.0, osgEarth::Units::DEGREES);//观察的角度
	em->setViewpoint(vp, 1.0);

    theApp.bNeedModify = FALSE;
}

三、运行效果如下：