osg 中鼠标拾取线段的端点和中点

//NeartestPointNodeVisitor.h
#pragma once
#include <osgMatrix>
#include <vector>
#include <osgNode>
#include <osgNodeVisitor>
#include <osgCamera>
#include <osgVec3>
#include <osgMatrixTransform>
#include <osgBoundingSphere>
#include <osgGroup>
#include <osgArray>
#include <osgGeometry>

/// 寻找离鼠标最近的点
class NearestPointNodeVisitor : public osg::NodeVisitor
{
public:
    // 构造函数
    NearestPointNodeVisitor(float x=0, float y=0, osg::Matrix m=osg::Matrix::identity(), bool b=false, float error=5);

    // 构造函数。VPW矩阵通过摄像机计算
    NearestPointNodeVisitor(float x, float y, osg::Camera* camera, bool b=false, float error=5);

    // 鼠标屏幕坐标 
    void setMouseXY(float x, float y);
    // 摄像机
    void setVPWmatrix(osg::Camera* camera);
    // VPWmatrix
    void setVPWmatrix(osg::Matrix m);
    void setPara(float x, float y, osg::Camera* camera, bool b);
    // 是否拾取中点
    void setComputeMidPoint(bool b);
    void setFindFalse();

    void apply( osg::Node& node );

    void findNearestPoint(const std::vector<osg::Vec3> points, const osg::Matrix &m);

    void apply( osg::Geode& node );

    bool getFind();
    //以下两个函数必须配合getFind()函数使用
    osg::Vec3 getAttachedPointWinCoord();
    osg::Vec3 getAttachedPoint();
private:
    /// 鼠标点击的窗口坐标
    float mouseX, mouseY;
    //视线与远近裁剪面的交点
    osg::Vec3 nearPoint, farPoint;
    /// 视图*投影*视口（窗口）矩阵的乘积矩阵
    osg::Matrix VPWmatrix;
    /// 吸附误差（单位为屏幕像素）
    float attachError;
    /// 被吸附的点（离鼠标最近的点）
    osg::Vec3 attachedPoint;
    /// 被吸附的点对应的窗口坐标
    osg::Vec3 attachedPointWinCoord;
    /// 被吸附点的深度值
    float depth;
    /// 是否找到可吸附点
    bool find;
    /// 每个geom中线段的中点
    std::map< osg::Geometry*, std::vector<osg::Vec3> > geom_map_midPoint; 
    /// 是否吸附线段的中点
    bool isMP;
};

//NeartestPointNodeVisitor.cpp
#include "NearestPointNodeVisitor.h"
#include <osgGeode>
#include <osgLineWidth>

// 计算点到直线的距离
// 返回值：点C到直线AB的最近距离；nearestPoint是直线AB上距离点C最近的点
template <class T>
float MyPointToLine(const T &C, const T &A, const T &B, T &nearestPoint)
{
    float minDistance=0;
    T ab = B-A;// 直线AB方向向量
    T ac = C-A;
    T n1 = ac^ab;//ac叉乘ab，得到平面ABC的法向量
    T n2 = ab^n1;//ab叉乘n1，得到平行于平面ABC且垂直AB的向量
    n2.normalize();//单位化
    minDistance = ac*n2;//AC在n2方向上的投影
    nearestPoint = C-n2*minDistance;
    return minDistance;
}
// 求一个点到线段的最近距离。算法来自于网络。
// 返回值：点C到线段AB的最近距离；nearestPoint是线段AB上距离点C最近的点
template <class T>
float MyPointToLineSegment(const T &C, const T &A, const T &B, T &nearestPoint)
{
    T ac = C-A;
    T ab = B-A;//线段所在直线向量

    float f = ac*ab;
    if ( f<0 )//C点在AB上的投影位于A的左侧
    {
        nearestPoint = A;
        return ac.length();
    }
    else if (f>ab*ab)//C点在AB上的投影位于B的右侧
    {
        nearestPoint = B;
        return (B-C).length();
    }
    else//C点在AB上的投影位于线段AB内部
    {
        float abLen2 = ab.length2();//ab长度的平方
        nearestPoint = A;
        float epsilon = 0.000001f;
        if ( abLen2 - 0.0f > epsilon )//ab长度不等于0，亦即A、B两点不重合
        {
            nearestPoint += ab*(f/abLen2);
        }
        return (nearestPoint-C).length();
    }
}

// 计算两条直线之间的距离
// E是直线AB上一点，F是直线CD上一点，线段EF是直线AB到直线CD距离最短的线段
// 返回值为线段EF的长度
template <class T>
float MyLineToLine(const T& A, const T &B, const T &C, const T &D, T &E, T &F)
{
    // 设CD在过AB且平行CD的平面π上的投影为C'D'，M、N为CD上两点，P、Q为C'D'上两点，
    // AM⊥CD，AP⊥C'D'，BN⊥CD，BQ⊥C'D'，可以证明P、Q分别为M、N在π上的投影
    // 并且有△AEP∽△BEQ，所以AE/EB=AP/BQ，计算出AP和BQ的长度即可
    float minDistance=0;
    T ab = B-A;
    T cd = D-C;
    T n = ab^cd;//叉乘。直线ab和cd公共垂直的直线方向向量
    float epsilon = 1e-6;
    if (n.length()>epsilon)//ab和cd不平行
    {
        n.normalize();//单位化n
        minDistance = n*(C-A);
    }
    return minDistance;
}

// 计算两条线段之间的距离
// E是线段AB上一点，F是线段CD上一点，线段EF是线段AB到线段CD距离最短的线段
// 返回值为线段EF的长度
template <class T>
float MyLineSegmentToLineSegment(const T& A, const T &B, const T &C, const T &D, T &E, T &F)
{
    // 设CD在过AB且平行CD的平面上的投影为C'D'，
    float minDistance=0;
    T ab = B-A;
    T cd = D-C;
    T n = ab^cd;//叉乘。直线ab和cd公共垂直的直线方向向量
    float epsilon = 1e-6;
    if (n.length()>epsilon)//ab和cd不平行
    {
        n.normalize();//单位化n
        //minDistance = n*(C-A)
    }
    return minDistance;
}

NearestPointNodeVisitor::NearestPointNodeVisitor(float x/*=0*/, float y/*=0*/, osg::Matrix m/*=osg::Matrix::identity()*/, bool b/*=false*/, float error/*=5*/) : 
NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
    mouseX(x),
    mouseY(y),
    VPWmatrix(m),
    attachError(error),//吸附误差
    depth(1),//默认在远裁剪面
    find(false),
    isMP(b)//是否吸附线段中点
{

}

// 构造函数。VPW矩阵通过摄像机计算
NearestPointNodeVisitor::NearestPointNodeVisitor(float x, float y, osg::Camera* camera, bool b/*=false*/, float error/*=5*/) : 
NodeVisitor(osg::NodeVisitor::TRAVERSE_ALL_CHILDREN),
    mouseX(x),
    mouseY(y),
    attachError(error),//吸附误差
    depth(1),//默认在远裁剪面
    find(false),
    isMP(b)//是否吸附线段中点
{
    osg::Matrix viewMatrix = camera->getViewMatrix();
    osg::Matrix projectionMatrix = camera->getProjectionMatrix();
    osg::Matrix windowMatrix = camera->getViewport()->computeWindowMatrix();
    VPWmatrix = viewMatrix*projectionMatrix*windowMatrix;

    //以下语句测试证明两种方法都能正确的计算视线和远近裁剪面的交点
    //osgManipulator::PointerInfo pi;
    //pi.setCamera(camera);
    //pi.setMousePosition(x,y);
    //osg::Vec3d nearP,farP;
    //pi.getNearFarPoints(nearP,farP);
    //osg::Vec3 n,f;
    //n = osg::Vec3(x,y,0)*osg::Matrix::inverse(VPWmatrix);
    //f = osg::Vec3(x,y,1)*osg::Matrix::inverse(VPWmatrix);

    nearPoint = osg::Vec3(mouseX,mouseY,0.0f)*osg::Matrix::inverse(VPWmatrix);
    farPoint = osg::Vec3(mouseX,mouseY,1.0f)*osg::Matrix::inverse(VPWmatrix);
}

// 鼠标屏幕坐标 
void NearestPointNodeVisitor::setMouseXY(float x, float y)
{ 
    mouseX=x; mouseY=y;
    nearPoint = osg::Vec3(mouseX,mouseY,0.0f)*osg::Matrix::inverse(VPWmatrix);
    farPoint = osg::Vec3(mouseX,mouseY,1.0f)*osg::Matrix::inverse(VPWmatrix);
}
// 摄像机
void NearestPointNodeVisitor::setVPWmatrix(osg::Camera* camera)
{
    osg::Matrix viewMatrix = camera->getViewMatrix();
    osg::Matrix projectionMatrix = camera->getProjectionMatrix();
    osg::Matrix windowMatrix = camera->getViewport()->computeWindowMatrix();
    VPWmatrix = viewMatrix*projectionMatrix*windowMatrix;
    nearPoint = osg::Vec3(mouseX,mouseY,0.0f)*osg::Matrix::inverse(VPWmatrix);
    farPoint = osg::Vec3(mouseX,mouseY,1.0f)*osg::Matrix::inverse(VPWmatrix);
}
// VPWmatrix
void NearestPointNodeVisitor::setVPWmatrix(osg::Matrix m)
{
    VPWmatrix=m;
    nearPoint = osg::Vec3(mouseX,mouseY,0.0f)*osg::Matrix::inverse(VPWmatrix);
    farPoint = osg::Vec3(mouseX,mouseY,1.0f)*osg::Matrix::inverse(VPWmatrix);
}
void NearestPointNodeVisitor::setPara(float x, float y, osg::Camera* camera, bool b)
{
    setMouseXY(x,y);
    setVPWmatrix(camera);
    nearPoint = osg::Vec3(mouseX,mouseY,0.0f)*osg::Matrix::inverse(VPWmatrix);
    farPoint = osg::Vec3(mouseX,mouseY,1.0f)*osg::Matrix::inverse(VPWmatrix);
    isMP=b;
}
// 是否拾取中点
void NearestPointNodeVisitor::setComputeMidPoint(bool b) { isMP=b;}
void NearestPointNodeVisitor::setFindFalse() { find=false;}

void NearestPointNodeVisitor::apply( osg::Node& node )
{
    if ( find )
    {
        return;
    }
    osg::NodePath np(getNodePath().begin(),getNodePath().end()-1);// 除去自身
    osg::Matrix m = osg::computeLocalToWorld(np);//当前节点所在的世界变换矩阵
    const osg::BoundingSphere &bs = node.getBound();
    osg::Vec3 center = bs.center();
    float radius = bs.radius();
    center=center*m;//转换成世界坐标
    float distance = MyPointToLine(center,nearPoint,farPoint,osg::Vec3());
    /// 检测模型包围球球心到视线的距离是否小于包围球半径，这样能减少计算量
    /// 同时还能保证很高的正确率（但不能达到100%，例如从球体外侧靠近球面上的点，
    /// 此时distance>radius，但是有可能球面上的点在拾取范围之内）
    if (distance<radius)
    {
        traverse( node );
    }
}

void NearestPointNodeVisitor::findNearestPoint(const std::vector<osg::Vec3> points, const osg::Matrix &m)
{
    for ( unsigned int i=0; i<points.size(); ++i )
    {
        osg::Vec3 currentPoint = points.at(i)*m;//转换成世界坐标
        osg::Vec3 winCoord = currentPoint*VPWmatrix;//窗口坐标
        if (/*winCoord.z()<depth&&*/(winCoord-osg::Vec3(mouseX,mouseY,0)).length()<attachError)
        {
            depth = winCoord.z();
            attachedPoint = currentPoint;
            attachedPointWinCoord = winCoord;
            find=true;
            break;// 找到第一个就算结束了
        }
    }
}

void NearestPointNodeVisitor::apply( osg::Geode& node )
{
    // 检测geode的包围球是否被射线穿过
    osg::Matrix m = osg::computeLocalToWorld(getNodePath());//当前叶子节点对应的世界变换矩阵
    const osg::BoundingSphere &bs = node.getBound();
    osg::Vec3 center = bs.center();
    center=center*m;//转换成世界坐标
    float radius = bs.radius();
    float distance = MyPointToLine(center,nearPoint,farPoint,osg::Vec3());
    if (distance>radius)
    {
        return;
    }

    osg::Geode::DrawableList drawableList = node.getDrawableList();
    osg::Geode::DrawableList::iterator it = drawableList.begin();
    for ( ; it != drawableList.end(); ++it)
    {
        osg::Geometry* geom = it->get()->asGeometry();
        if ( !geom )
        {
            continue;
        }
        // 检测geom的包围盒是否被射线穿过
        const osg::BoundingBox &bs = geom->getBound();
        osg::Vec3 center = bs.center();
        center=center*m;//转换成世界坐标
        float radius = bs.radius();
        float distance = MyPointToLine(center,nearPoint,farPoint,osg::Vec3());
        // 如果射线没穿过geom，计算下一个geom
        if (distance>radius)
        {
            continue;
        }

        // 遍历geom的全部点
        osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geom->getVertexArray());
        findNearestPoint(vertices->asVector(), m);
        // 拾取中点
        if ( isMP )
        {
            std::map<osg::Geometry*, std::vector<osg::Vec3>>::iterator itr = geom_map_midPoint.find(geom);
            // geom的中点集合还没计算
            if ( itr == geom_map_midPoint.end() )
            {
                //osg::TemplatePrimitiveFunctor<MidPointOfGeom> midPoint;
                //geom->accept(midPoint);
                //geom_map_midPoint.insert(std::map<osg::Geometry*, std::vector<osg::Vec3>>::value_type
                // (geom, midPoint.vertex));
                osg::Geometry::PrimitiveSetList psList = geom->getPrimitiveSetList();
                osg::Geometry::PrimitiveSetList::iterator psListItr = psList.begin();
                std::vector<osg::Vec3> midPoints;
                for ( ; psListItr != psList.end(); psListItr++ )
                {
                    osg::PrimitiveSet *ps = psListItr->get();
                    GLenum mode = ps->getMode();
                    switch( ps->getType() )
                    {
                    case osg::PrimitiveSet::DrawArraysPrimitiveType:
                        {
                            switch( mode )
                            {
                            case osg::PrimitiveSet::LINES:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::LINE_STRIP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::LINE_LOOP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLES:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLE_STRIP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLE_FAN:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::QUADS:
                                {
                                    break;
                                }
                            }
                            break;
                        }
                    case osg::PrimitiveSet::DrawElementsUBytePrimitiveType:
                        {
                            break;
                        }
                    case osg::PrimitiveSet::DrawElementsUShortPrimitiveType:
                        {
                            osg::DrawElementsUShort* deus = dynamic_cast<osg::DrawElementsUShort*>(ps);
                            const unsigned indexNum = deus->getNumIndices();
                            switch( mode )
                            {
                            case osg::PrimitiveSet::LINES:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::LINE_STRIP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::LINE_LOOP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLES:
                                {
                                    typedef std::map< unsigned int, std::set<unsigned int> > INT_SET;
                                    INT_SET start_end;
                                    for (unsigned int i=0; i<indexNum; i+=3) {
                                        // 对三个索引排序，最终a<=b<=c
                                        unsigned short a=osg::minimum(deus->at(i),osg::minimum(deus->at(i+1),deus->at(i+2)));
                                        unsigned short c=osg::maximum(deus->at(i),osg::maximum(deus->at(i+1),deus->at(i+2)));
                                        unsigned short b=(deus->at(i)+deus->at(i+1)+deus->at(i+2)-a-c);
                                        //查找(a,b)，(a,c)是否已经计算过
                                        INT_SET::iterator itr = start_end.find(a);
                                        if ( itr==start_end.end() )//没有a组
                                        {
                                            std::set<unsigned int> aset;
                                            aset.insert(b);
                                            aset.insert(c);
                                            start_end.insert(make_pair(a,aset));
                                            // 计算中点
                                            midPoints.push_back((vertices->at(a)+vertices->at(b))/2);
                                            midPoints.push_back((vertices->at(a)+vertices->at(c))/2);
                                        }
                                        else
                                        {
                                            if (itr->second.count(b)==0)//有a组但没有(a,b)对
                                            {
                                                start_end[a].insert(b);
                                                midPoints.push_back((vertices->at(a)+vertices->at(b))/2);
                                            }
                                            if (itr->second.count(c)==0)//有a组但没有(a,c)对
                                            {
                                                start_end[a].insert(c);
                                                midPoints.push_back((vertices->at(a)+vertices->at(c))/2);
                                            }
                                        }
                                        //查找(b,c)是否已经计算过
                                        itr = start_end.find(b);
                                        if ( itr==start_end.end() )//没有b组
                                        {
                                            std::set<unsigned int> bset;
                                            bset.insert(c);
                                            start_end.insert(make_pair(b,bset));
                                            midPoints.push_back((vertices->at(b)+vertices->at(c))/2);
                                        }
                                        else
                                        {
                                            if (itr->second.count(c)==0)//有b组但没有(b,c)对
                                            {
                                                start_end[b].insert(c);
                                                midPoints.push_back((vertices->at(b)+vertices->at(c))/2);
                                            }
                                        }
                                    }
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLE_STRIP:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::TRIANGLE_FAN:
                                {
                                    break;
                                }
                            case osg::PrimitiveSet::QUADS:
                                {
                                    break;
                                }
                            }
                            break;
                        }
                    case osg::PrimitiveSet::DrawElementsUIntPrimitiveType:
                        {
                            break;
                        }
                    }// END ps->getType()
                }//END for psList
                geom_map_midPoint.insert(make_pair(geom, midPoints));
                findNearestPoint(midPoints, m);
            }//END if ( itr == geom_map_midPoint.end() )
            else
            {
                findNearestPoint(itr->second, m);
            }
        }    // END if ( isMP )
    }//     for ( ; it != drawableList.end(); ++it)
}

bool NearestPointNodeVisitor::getFind()
{
    return find;
}
//以下两个函数必须配合getFind()函数使用
osg::Vec3 NearestPointNodeVisitor::getAttachedPointWinCoord()
{
    return attachedPointWinCoord;
}
osg::Vec3 NearestPointNodeVisitor::getAttachedPoint()
{
    return attachedPoint;
}