方向向量转欧拉角

转载原始帖子：https://www.jianshu.com/p/1b4f310fb883

本节就一个功能，加载一个导弹，让其在上一节的基础上，从发射架那里打到地面站那里。

 

 

image.png

具体实现

这里我们输入的是一连串的经纬高，组成导弹的路径。然后生成一个animationPath就可以了。第10节 实例-双击跑过去操作器 在这一节也用了animationPath，animationPath最关键的参数是位置、朝向，位置好说，关键点就是位置。关键在朝向。有了位置、朝向、时间直接就可以生成animationPath。

要计算导弹在一个点的朝向确非易事，要先搞明白一件事情：就是导弹在建模时有个方向，这是导弹的局部坐标。将其放在地球上是使用的这样的方法：

    osg::MatrixTransform* mt = new osg::MatrixTransform;
    mt->addChild(LodAutoMode(fileName, radioSize));

    osg::Matrixd mts;
    _em->computeLocalToWorldTransformFromLatLongHeight(osg::inDegrees(LLH.y()), osg::inDegrees(LLH.x()), LLH.z(), mts);
    mt->setMatrix(mts);

我们要知道mts这个矩阵，将物体从世界坐标系的(0,0,0)给移到地表，除了改变了位置以外，还改变了朝向。这个要想象一下，假如平移则放在地表的角度是不对的。拿坐标轴来比一下，在世界坐标下这个轴是这样的（Y轴正北，Z轴从地心连，X轴正东）：

 

本例中计算朝向使用了一个函数叫做：void GetFlyPosture(osg::Vec3d First, osg::Vec3d Second, double& PitchAngle, double& yAngleHengGun, double& YawAngle)
给定起点、终点，然后计算出导弹的俯仰角、横滚角、航向角。这里详细的判断了起点和终点的经纬高之间的关系，读者可以看一下理解一下，比如经纬度没有变，只高度变化，则就是顺着地表垂直向上发射

作者：杨石兴
链接：https://www.jianshu.com/p/1b4f310fb883
来源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

#include <osgViewer/Viewer>
#include <osgEarth/EarthManipulator>
#include <osgDB/ReadFile>
#include <osg/Geode>
#include <osg/MatrixTransform>
#include <osg/LineWidth>
#include <osg/LineStipple>
#include <osg/AutoTransform>
#include <osg/AnimationPath>
#include <osg/ImageSequence>
#include <osg/Depth>

#include <osgParticle/Particle>
#include <osgParticle/ParticleSystem>
#include <osgParticle/ModularEmitter>
#include <osgParticle/RandomRateCounter>
#include <osgParticle/SectorPlacer>
#include <osgParticle/ModularProgram>
#include <osgParticle/AccelOperator>
#include <osgParticle/ParticleSystemUpdater>

//全局椭球体，用于经纬度坐标与XYZ坐标互相转换
osg::EllipsoidModel* _em = new osg::EllipsoidModel;
osg::Node* _lanFang = nullptr;

class FindNodeVisitor : public osg::NodeVisitor
{
public:
    FindNodeVisitor() :osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN)
    {
    }

    virtual void apply(osg::Node& node)
    {
        if (node.getName() == _showName)
        {
            node.asGroup()->getChild(0)->setNodeMask(~0);
        }

        if (node.getName() == _hideName)
        {
            node.asGroup()->getChild(0)->setNodeMask(0);
        }

        traverse(node);
    }

    std::string _showName;
    std::string _hideName;
};

class apCtrl : public osg::NodeCallback
{
public:    
    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        //先获取当前路径更新回调
        osg::AnimationPathCallback* apc = dynamic_cast<osg::AnimationPathCallback*>(node->getUpdateCallback());
        if (nullptr != apc)
        {
            //判断推演是否结束
            if (apc->getAnimationPath()->getPeriod() <= (apc->getAnimationTime() + 0.0001))
            {
                //推演结束，导弹消失，爆炸要爆
                FindNodeVisitor fnv;
                fnv._showName = "Explosion";
                fnv._hideName = "BaseMode";
                node->accept(fnv);

                //蓝方的房子也要隐藏
                _lanFang->accept(fnv);
            }
        }
        //不需要再往下处理了，就处理这一个结点就行
        //traverse(node, nv);
    }

};

//创建爆炸效果
osg::Node* explosion(int size)
{
    osg::ImageSequence* imageSequence = new osg::ImageSequence;
    for (int i = 1; i <= 121; i++)
    {
        std::stringstream buf;
        buf << "./image/Explosion" << std::setw(4) << std::setfill('0') << i << ".png";
        std::string imageName = buf.str();

        osg::ref_ptr<osg::Image> image = osgDB::readImageFile(imageName);
        if (image.valid())
        {
            imageSequence->addImage(image.get());
        }
    }

    unsigned int maxNum = imageSequence->getNumImageData();
    imageSequence->setLength(double(maxNum) * (1.0 / 30.0));

    //
    imageSequence->setLoopingMode(osg::ImageStream::LOOPING);

    osg::Texture2D* texture = new osg::Texture2D;
    texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
    texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
    texture->setWrap(osg::Texture::WRAP_R, osg::Texture::REPEAT);
    texture->setResizeNonPowerOfTwoHint(false);
    texture->setImage(imageSequence);

    osg::Billboard* center = new osg::Billboard();
    center->setMode(osg::Billboard::POINT_ROT_EYE);
    center->addDrawable(osg::createTexturedQuadGeometry(osg::Vec3(-size / 2, 0.0, -size / 2), osg::Vec3(size, 0.0f, 0.0), osg::Vec3(0.0f, 0.0f, size)));

    osg::StateSet* stateset2 = center->getOrCreateStateSet();
    stateset2->setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);
    stateset2->setMode(GL_BLEND, osg::StateAttribute::ON | osg::StateAttribute::PROTECTED | osg::StateAttribute::OVERRIDE);
    stateset2->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::PROTECTED | osg::StateAttribute::OVERRIDE);

    osg::Depth* dp = new osg::Depth();
    dp->setWriteMask(false);
    center->getOrCreateStateSet()->setAttribute(dp, osg::StateAttribute::ON | osg::StateAttribute::PROTECTED | osg::StateAttribute::OVERRIDE);

    imageSequence->play();
    imageSequence->rewind();

    return center;
}

void RadianLLH2XYZ(const osg::Vec3d& vecLLH, osg::Vec3d& vecXYZ)
{
    _em->convertLatLongHeightToXYZ(
        vecLLH.y(), vecLLH.x(), vecLLH.z(), vecXYZ.x(), vecXYZ.y(), vecXYZ.z());
}

void DegreeLLH2XYZ(const osg::Vec3d& vecLLH, osg::Vec3d& vecXYZ)
{
    osg::Vec3d vecRadianLLH(osg::DegreesToRadians(vecLLH.x()), osg::DegreesToRadians(vecLLH.y()), vecLLH.z());
    return RadianLLH2XYZ(vecRadianLLH, vecXYZ);
}

double GetDistance(const osg::Vec3d& start, const osg::Vec3d& end)
{
    return (end - start).length();
}

bool FLOAT_EQUAL(float l, float r)
{
    if (0.000001 >= (l - r) && -0.000001 <= (l - r))
    {
        return true;
    }
    return false;
}

void GetFlyPosture(osg::Vec3d First, osg::Vec3d Second, double& PitchAngle, double& yAngleHengGun, double& YawAngle)
{
    //记录第一个点转换为世界坐标后的值
    osg::Vec3d FirstXYZ;
    //记录第二个点转换为世界坐标后的值
    osg::Vec3d SecondXYZ;

    DegreeLLH2XYZ(First, FirstXYZ);
    DegreeLLH2XYZ(Second, SecondXYZ);

   
    //首先计算俯仰角
    if (FLOAT_EQUAL(First.z(), Second.z()))
    {
        PitchAngle = 0;
    }
    else
    {
        double distance = GetDistance(FirstXYZ, SecondXYZ);
        double detal = First.z() - Second.z();

        while (abs(detal) >= abs(distance))
        {
            if (distance > 0)
            {
                distance = distance + 0.001;
            }
            else
            {
                distance = distance - 0.001;//因为经纬高都是double类型的计算距离等于0时可能是取的近似值。
            }
        }
        PitchAngle = -asin(detal / distance);
    }

   
    //计算航向角
    //如果第一个点和第二个点的纬度相同，则航向角为90度或者-90度
    if (FLOAT_EQUAL(First.y(), Second.y()))
    {
        double detal = Second.x() - First.x();//计算经度差
        if (detal < -180 || (detal > 0 && detal < 180))
        {
            //在其右侧180度内
            YawAngle = osg::DegreesToRadians(-90.0);//逆时针转为正向，所以在右侧时转的角度为负值。
        }
        else
        {
            //在其左侧180度内
            YawAngle = osg::DegreesToRadians(90.0);
        }
    }
    //如果第一个点的经度和第二个点的经度相同则航向角为0度或者180度
    else if (FLOAT_EQUAL(First.x(), Second.x()))
    {
        double detal = Second.y() - First.y();
        if (detal > 0)
        {
            YawAngle = osg::DegreesToRadians(0.0);
        }
        else
        {
            YawAngle = osg::DegreesToRadians(180.0);
        }
    }
    //第一个点和第二个点的经度和维度都不同，此种情况下只考虑xy坐标就可以了不用考虑z即只在XOY平面做计算就可以了
    else
    {
        double detalX = Second.x() - First.x();

        osg::Vec3d Second1(Second.x(), Second.y(), First.z());
        osg::Vec3d Second1XYZ;
        DegreeLLH2XYZ(Second1, Second1XYZ);

        double dDistance1 = GetDistance(FirstXYZ, Second1XYZ);

        osg::Vec3d three(First.x(), Second.y(), Second1.z());
        osg::Vec3d threeXYZ;
        DegreeLLH2XYZ(three, threeXYZ);
        double dDistance2 = GetDistance(FirstXYZ, threeXYZ);

        while (abs(dDistance2) >= abs(dDistance1))
        {
            if (dDistance1 > 0)
            {
                dDistance1 = dDistance1 + 0.001;
            }
            else
            {
                dDistance1 = dDistance1 - 0.001;
            }
        }

        double Angle = acos(dDistance2 / dDistance1);
        //如果第二个点在第一个点的右边
        if (detalX < -180 || (detalX > 0 && detalX < 180))
        {
            if (Second.y() > First.y())
            {
                YawAngle = -Angle;
            }
            else
            {
                YawAngle = -(osg::PI - Angle);
            }
        }
        //第二点在第一个点的左侧
        else
        {
            if (Second.y() > First.y())
            {
                YawAngle = Angle;
            }
            else
            {
                YawAngle = osg::PI - Angle;
            }
        }
    }
    //std::cout<<osg::RadiansToDegrees(YawAngle)<<std::endl;
}

//注意这里的osg::Vec3 xyz是纬、经、高
osg::AnimationPath* creatAnimation(osg::Vec3dArray* arrayPosition, double time)
{
    osg::AnimationPath* ap = new osg::AnimationPath;
    ap->setLoopMode(osg::AnimationPath::NO_LOOPING);

    osg::Matrix posMatrix;
    osg::Matrix matrix;

    double dPitch = 0.0, dRoll = 0.0, dYaw = 0.0;

    if (arrayPosition->size() > 1)
    {
        _em->computeLocalToWorldTransformFromLatLongHeight(
                osg::DegreesToRadians(arrayPosition->at(0).y()),
                osg::DegreesToRadians(arrayPosition->at(0).x()),
                arrayPosition->at(0).z(), matrix);

        GetFlyPosture(arrayPosition->at(0), arrayPosition->at(1), dPitch, dRoll, dYaw);
    }

    osg::Matrix adjustPosture = osg::Matrix::rotate(dPitch, osg::X_AXIS, 0.0, osg::Y_AXIS, dYaw, osg::Z_AXIS);

    posMatrix = adjustPosture * matrix;

    ap->insert(0.0, osg::AnimationPath::ControlPoint(posMatrix.getTrans(), posMatrix.getRotate()));
    double dAveTime = time / (arrayPosition->size() - 1.0);
    for (unsigned int first = 0, second = first + 1; second < arrayPosition->size(); first++, second++)
    {
        _em->computeLocalToWorldTransformFromLatLongHeight(
                osg::DegreesToRadians(arrayPosition->at(second).y()),
                osg::DegreesToRadians(arrayPosition->at(second).x()),
                arrayPosition->at(second).z(), matrix);

        double dPitch = 0.0, dRoll = 0.0, dYaw = 0.0;
        GetFlyPosture(arrayPosition->at(first), arrayPosition->at(second), dPitch, dRoll, dYaw);

        osg::Matrix adjustPosture = osg::Matrix::rotate(dPitch, osg::X_AXIS, 0.0, osg::Y_AXIS, dYaw, osg::Z_AXIS);

        posMatrix = adjustPosture * matrix;

        ap->insert(dAveTime * (first + 1),
            osg::AnimationPath::ControlPoint(
                posMatrix.getTrans(),
                posMatrix.getRotate()));
    }

    return ap;
}

osg::Node* LodAutoMode(std::string fileName, float radioSize)
{
    osg::MatrixTransform* tm0 = new osg::MatrixTransform;
    tm0->addChild(osgDB::readNodeFile(fileName));
    tm0->setMatrix(osg::Matrix::scale(osg::Vec3(radioSize, radioSize, radioSize)));
    tm0->setName("BaseMode");

    //之所以上面要加一层，是隐藏之后NodeVisitor就访问不到了，就无
    //法再被遍历到
    osg::Group* exTm0 = new osg::Group;
    exTm0->setName("BaseMode");
    exTm0->addChild(tm0);

    osg::AutoTransform* at = new osg::AutoTransform;
    at->addChild(exTm0);

    //给每个模型都加个爆炸效果，之所以上面要加一层，是隐藏之后NodeVisitor就访问不到了，就无
    //法再被遍历到
    osg::Group* exRoot = new osg::Group;
    exRoot->setName("Explosion");
    osg::Node* ex = explosion(200.0);
    exRoot->addChild(ex);
    ex->setNodeMask(0);

    at->addChild(exRoot);

    at->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF | osg::StateAttribute::OVERRIDE);
    at->setAutoScaleToScreen(true);
    at->setMinimumScale(100.0);
    at->setMaximumScale(10000);
    at->setAutoScaleTransitionWidthRatio(0.0);
    return at;
}

osg::Node* LodAutoModel(std::string fileName, osg::Vec3 LLH, float radioSize)
{
    osg::MatrixTransform* mt = new osg::MatrixTransform;
    mt->addChild(LodAutoMode(fileName, radioSize));

    osg::Matrixd mts;
    _em->computeLocalToWorldTransformFromLatLongHeight(osg::inDegrees(LLH.y()), osg::inDegrees(LLH.x()), LLH.z(), mts);
    mt->setMatrix(mts);

    return mt;
}

//起点经纬度，终点经纬度，中间最高点，来创建一个简单的曲线，再加个导弹来飞循环飞一下
osg::Group* BuildScene(osg::Vec3 fromLLH, osg::Vec3 toLLH, float topH)
{
    osg::Group* sceneRoot = new osg::Group;
    
    //给线前面加一个mt是为了防止大坐标抖动
    osg::MatrixTransform* mtLine = new osg::MatrixTransform;
    sceneRoot->addChild(mtLine);

    osg::Geode* line = new osg::Geode;
    mtLine->addChild(line);

    osg::Geometry* lineGeom = new osg::Geometry;
    line->addDrawable(lineGeom);

    //线的宽度设置成5
    osg::LineWidth* lw = new osg::LineWidth(3.0);
    lineGeom->getOrCreateStateSet()->setAttributeAndModes(lw, osg::StateAttribute::ON);
    lineGeom->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);

    //设置成点画线
    osg::LineStipple* ls = new osg::LineStipple(1, 0x00FF);
    lineGeom->getOrCreateStateSet()->setAttributeAndModes(ls, osg::StateAttribute::ON);

    osg::Vec3Array* vertex = new osg::Vec3Array;
    lineGeom->setVertexArray(vertex);

    //线的颜色设置成高级灰
    osg::Vec4Array* color = new osg::Vec4Array;
    color->push_back(osg::Vec4(0.8, 0.8, 0.8, 1.0));
    lineGeom->setColorArray(color, osg::Array::BIND_OVERALL);

    //从起点到终点的经纬度，采用简单插值算法，插上100个点
    float deltaLat = (toLLH.x() - fromLLH.x()) / 100;
    float deltaLon = (toLLH.y() - fromLLH.y()) / 100;
    //高度变化前半程
    float deltaHF = (topH - fromLLH.z()) / 50;
    //高度变化后半程
    float deltaHT = (toLLH.z() - topH) / 50;

    //防止大坐标抖动，将所有顶点的值都减动一个fromLLH
    osg::Vec3d fromV;
    _em->convertLatLongHeightToXYZ(osg::inDegrees(fromLLH.y()), osg::inDegrees(fromLLH.x()), fromLLH.z(), fromV.x(), fromV.y(), fromV.z());
    mtLine->setMatrix(osg::Matrix::translate(fromV));

    //关键点的经纬度形式
    osg::Vec3dArray* llhArray = new osg::Vec3dArray;

    for (int i = 0; i < 100; i++)
    {
        osg::Vec3 tempPoint = fromLLH + osg::Vec3(deltaLat*i, deltaLon*i, deltaHF*i);
        if (i > 49)//到中间了要往下了
        {
            tempPoint.z() = topH + deltaHT*(i-49);
        }

        llhArray->push_back(tempPoint);

        osg::Vec3d tempV;
        _em->convertLatLongHeightToXYZ(osg::inDegrees(tempPoint.y()), osg::inDegrees(tempPoint.x()), tempPoint.z(), tempV.x(), tempV.y(), tempV.z());
        tempV -= fromV; //防止大坐标抖动
        vertex->push_back(tempV);
    }

    lineGeom->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, vertex->size()));

    osg::AnimationPath* ap = creatAnimation(llhArray, 30.0);
    osg::MatrixTransform* mtAp = new osg::MatrixTransform;
    mtAp->addUpdateCallback(new osg::AnimationPathCallback(ap));
    //下面这个callback用来负责控制导弹是否击中目标（其实就是路径播完）
    mtAp->addUpdateCallback(new apCtrl());
    mtAp->addChild(LodAutoMode("huojianend.ive", 35.0));
    sceneRoot->addChild(mtAp);

    return sceneRoot;
}

int main(int argc, char** argv)
{
    osgEarth::initialize();
    osgViewer::Viewer viewer;
    viewer.setCameraManipulator(new osgEarth::Util::EarthManipulator);

    osg::Group* root = new osg::Group;
    root->addChild(BuildScene(osg::Vec3(121.50, 25.04, 100), osg::Vec3(131.66, 33.22, 100), 100000));
    root->addChild(osgDB::readNodeFile("simple.earth"));
    root->addChild(LodAutoModel("RedCar.ive", osg::Vec3(121.50, 25.04, 100), 5.0));

    _lanFang = LodAutoModel("house.ive", osg::Vec3(131.66, 33.22, 100), 0.08);
    root->addChild(_lanFang);

    viewer.setSceneData(root);
    return viewer.run();
}