临时算法文件

// 二维点转三维函数[核心]
// 输入参数 : 转换平面上的点数组,二维点pt2DIn
// 输出参数 : pt3DOut
// 返回值 : 成功返回true,失败返回false
bool CGMap2DTo3DDlg::Point2Dto3D_Ext2(CArray<POINT2D3D,POINT2D3D> &aryPlanePts,POINT2D pt2DIn,POINT3D &pt3DOut)
{
    if(aryPlanePts.GetCount() < 3) return false;

    // 计算转换平面法向(ra,rb,rc)
    POINT3D A = aryPlanePts[0].pt3d;
    POINT3D B = aryPlanePts[1].pt3d;
    POINT3D C = aryPlanePts[2].pt3d;

    //方向矢量
    double bcx = B.X - C.X;
    double bcy = B.Y - C.Y;
    double bcz = B.Z - C.Z;

    double bax = B.X - A.X;
    double bay = B.Y - A.Y;
    double baz = B.Z - A.Z;

    //平面法矢量
    double ra,rb,rc;
    ra = bcy*baz - bay*bcz;
    rb = bax*bcz - bcx*baz;
    rc = bcx*bay - bax*bcy;   
    normalize(ra,rb,rc);

#if 0
    double rd = -(ra*aryPlanePts[0].pt3d.X + rb*aryPlanePts[0].pt3d.Y + rc*aryPlanePts[0].pt3d.Z);
    // 转换
    Point2Dto3D_Ext2(ra,rb,rc,rd,pt2DIn,pt3DOut);
#else// x,y成比例，z按照面法向与面上任意矢量点积为0来计算 2010.6.21
    double L_minx,L_maxx,L_miny,L_maxy;
    L_minx = L_maxx = aryPlanePts[0].pt2d.X;
    L_miny = L_maxy = aryPlanePts[0].pt2d.Y;
    double D_minx,D_maxx,D_miny,D_maxy;
    D_minx = D_maxx = aryPlanePts[0].pt3d.X;
    D_miny = D_maxy = aryPlanePts[0].pt3d.Y;
    double D_minz,D_maxz;
    D_minz = D_maxz = aryPlanePts[0].pt3d.Z;
    for (int i = 1; i < aryPlanePts.GetCount(); i++)
    {
        if (L_minx > aryPlanePts[i].pt2d.X)
        {
            L_minx = aryPlanePts[i].pt2d.X;
            D_minx = aryPlanePts[i].pt3d.X;
        }

        if (L_miny > aryPlanePts[i].pt2d.Y)
        {
            L_miny = aryPlanePts[i].pt2d.Y;       
            D_minz = aryPlanePts[i].pt3d.Z;
        }

        if (L_maxx < aryPlanePts[i].pt2d.X)
        {
            L_maxx = aryPlanePts[i].pt2d.X;
            D_maxx = aryPlanePts[i].pt3d.X;
        }

        if (L_maxy < aryPlanePts[i].pt2d.Y)
        {
            L_maxy = aryPlanePts[i].pt2d.Y;
            D_maxz = aryPlanePts[i].pt3d.Z;
        }

        if (D_miny > aryPlanePts[i].pt3d.Y)        
            D_miny = aryPlanePts[i].pt3d.Y;
        if (D_maxy < aryPlanePts[i].pt3d.Y)        
            D_maxy = aryPlanePts[i].pt3d.Y;
    }

    if ( (L_maxx - L_minx) != 0)
        pt3DOut.X = ( (pt2DIn.X-L_minx)/(L_maxx-L_minx) ) * (D_maxx-D_minx) + D_minx;
    else// 所有控制点X坐标一致
        pt3DOut.X = D_minx;

    double dy = L_maxy-L_miny;
    if ( dy != 0 )
        pt3DOut.Z = ( (pt2DIn.Y-L_miny)/(L_maxy-L_miny) ) * (D_maxz-D_minz) + D_minz;
    else
        pt3DOut.Z = A.Z;

    if (rb != 0)
    {
        pt3DOut.Y = (ra * (A.X - pt3DOut.X) + rc * (A.Z - pt3DOut.Z) + rb * A.Y)/rb;
    }
    else// 垂直Y轴
        pt3DOut.Y = A.Y;
    //if ( (L_maxy - L_miny) != 0)
    //    pt3DOut.Y = ( (pt2DIn.Y-L_miny)/(L_maxy-L_miny) ) * (D_maxy-D_miny) + D_miny;
    //else// 所有控制点Y坐标一致
    //    pt3DOut.Y = D_miny;

    //if(rc != 0)
    //{
    //    pt3DOut.Z = (ra*A.X + rb*A.Y + rc*A.Z - ra*pt3DOut.X - rb*pt3DOut.Y)/rc;
    //}
    ////垂直Z轴，不会出现 - 会出现
    //else
    //{
    //    //存在问题

    //    //距离不变性
    //    //pt3DOut.Z = sqrt( (pt2DIn.X - A.X)*(pt2DIn.X - A.X) + (pt2DIn.Y - A.Y)*(pt2DIn.Y - A.Y) ) - A.Z;

    //    // ml 改为直接使用某点的z值 2010.6.8
    //    //pt3DOut.Z = A.Z;
    //   
    //    // 沿Y轴方向进行Z值变化 - 针对向下的钻孔情况剖面
    //    double dy = D_maxy-D_miny;
    //    if ( dy != 0 )
    //        pt3DOut.Z = (pt3DOut.Y-D_miny)*(D_maxz-D_minz)/dy + D_minz;
    //    else
    //        pt3DOut.Z = A.Z;
    //}
#endif
    return true;
}

// 二维线转三维函数[核心]
// 输入参数 : 线的端点Ln_pt1,Ln_pt2,隐含参数控制点数组m_ctrlPointsArray
// 输出参数 : 转换后的交点数组tempArray
// 返回值 : 成功返回true,失败返回false
bool CGMap2DTo3DDlg::Line2Dto3D_Ext(POINT2D Ln_pt1,POINT2D Ln_pt2,CArray<POINT2D3D,POINT2D3D> &tempArray)
{
    //CArray<POINT2D3D,POINT2D3D> tempArray;
    // 如果tempArray数组内容不为空，清空之
    if (tempArray.GetSize() > 0)
        tempArray.RemoveAll();

    POINT2D3D ptInter;
    int polyid1,polyid2;
    POINT2D3D ln_node0,ln_node1;
    ln_node0.pt2d.X = Ln_pt1.X;
    ln_node0.pt2d.Y = Ln_pt1.Y;
    ln_node1.pt2d.X = Ln_pt2.X;
    ln_node1.pt2d.Y = Ln_pt2.Y;

    double l_minx = (Ln_pt1.X < Ln_pt2.X) ? Ln_pt1.X : Ln_pt2.X;
    double l_miny = (Ln_pt1.Y < Ln_pt2.Y) ? Ln_pt1.Y : Ln_pt2.Y;
    double l_maxx = (Ln_pt1.X > Ln_pt2.X) ? Ln_pt1.X : Ln_pt2.X;
    double l_maxy = (Ln_pt1.Y > Ln_pt2.Y) ? Ln_pt1.Y : Ln_pt2.Y;

    bool bSucc1 = PointInValidateRgn(ln_node0,polyid1);
    bool bSucc2 = PointInValidateRgn(ln_node1,polyid2);
    //if (bSucc1)    // 添加第一个点（起始点）
    // 首点必须添加做为排序基准点
    tempArray.Add(ln_node0);
    double maxx,maxy,minx,miny;
    maxx = maxy = -999999999999;
    minx = miny = 999999999999;
    if ( (polyid1 == polyid2) && (polyid1 != -1))
    {// 两点都在同一个poly中,并且不能都在可转换区域外
        // 由于判断点在多边形内PointInValidateRgn时已经将在转换范围内的点二维坐标转为三维，所以不必再次转换了
        // 对于两个点都落在转换区域外的line需要计算线与区域交点
    }
    else
    {// 两点不在一个转换平面内，需要计算一系列的交点,遍历所有的边计算交点
        CArray<POINT2D3D,POINT2D3D> poly;
        for (long i = 0; i < m_ctrlpolyIndexArray.GetSize(); i++)
        {
            int index = m_ctrlpolyIndexArray.GetAt(i);
            if (index != -1)
            {
                poly.Add(m_ctrlPointsArray[index]);
                if (minx > m_ctrlPointsArray[index].pt2d.X) minx = m_ctrlPointsArray[index].pt2d.X;
                if (miny > m_ctrlPointsArray[index].pt2d.Y) miny = m_ctrlPointsArray[index].pt2d.Y;
                if (maxx < m_ctrlPointsArray[index].pt2d.X) maxx = m_ctrlPointsArray[index].pt2d.X;
                if (maxy < m_ctrlPointsArray[index].pt2d.Y) maxy = m_ctrlPointsArray[index].pt2d.Y;
            }
            else
            {// -1表示结束了一个poly定义
                long count = poly.GetCount();               
                double r_minx = max(minx, l_minx);
                double r_miny = max(miny, l_miny);
                double r_maxx = min(maxx, l_maxx);
                double r_maxy = min(maxy, l_maxy);
                if (r_minx <= r_maxx && r_miny <= r_maxy)
                {//两矩形相交
                    for (long j = 0; j < count-1; j++)
                    {                       
                        if (IntersectLineLine(poly[j].pt2d,poly[j+1].pt2d,Ln_pt1,Ln_pt2,ptInter.pt2d) == 1)
                        {// 相交有交点ptInter ，当交点在poly区域内时，此点可以被添加到交点数组中
                            // 先用交点是否在线段ln_node0,ln_node1之间简单判断需不需要进一步判断点在poly多边形内，如果在则做进一步判断
                            // 这样做有助于加快判断速度，节约时间
                            double resX = (ptInter.pt2d.X-ln_node0.pt2d.X)*(ptInter.pt2d.X-ln_node1.pt2d.X);
                            double resY = (ptInter.pt2d.Y-ln_node0.pt2d.Y)*(ptInter.pt2d.Y-ln_node1.pt2d.Y);
                            if (resX <= ZERO && resY <= ZERO)
                            {// 交点在线段内,添加入交点数组并排序(注意不能将不在线段内得点加入交点集中)       
                                //if (PointInValidateRgn(ptInter,polyid1) == true)//交点在转换区域内    错的bug，应该是在poly中    2010.6.23

                                //resX = (ptInter.pt2d.X-minx)*(ptInter.pt2d.X-maxx);
                                //resY = (ptInter.pt2d.Y-miny)*(ptInter.pt2d.Y-maxy);

                                resX = (ptInter.pt2d.X-poly[j].pt2d.X)*(ptInter.pt2d.X-poly[j+1].pt2d.X);
                                resY = (ptInter.pt2d.Y-poly[j].pt2d.Y)*(ptInter.pt2d.Y-poly[j+1].pt2d.Y);
                                if (resX <= ZERO && resY <= ZERO)
                                {
                                    //bool bIn = IsPointInPolygon(poly,ptInter.pt2d);
                                    //if (bIn) // 在多边形内，返回真值
                                    {                                       
                                        // 二维点转三维，直接存储再ptIn.pt3d中 - 如果poly的点不够3个返回失败
                                        if (Point2Dto3D_Ext2(poly,ptInter.pt2d,ptInter.pt3d))
                                            AddToSortArray(tempArray,ptInter);                               
                                    }
                                }
                            }                       
                        }
                    }
                }
                poly.RemoveAll();

                maxx = maxy = -999999999999;
                minx = miny = 999999999999;
            }
        }
    }

    // 如果首点不在可转化区域内，移除首点
    if (!bSucc1) tempArray.RemoveAt(0);

    if (bSucc2)    // 如果尾点在可转化区域添加最后一个点(尾点)
        tempArray.Add(ln_node1);

    return true;
}

// 判断两POINT2D点是否同位置
bool CGMap2DTo3DDlg::IsEqual(POINT2D pt1,POINT2D pt2)
{
    if ( fabs(pt1.X-pt2.X) < ZERO && fabs(pt1.Y-pt2.Y) < ZERO)
        return true;
    return false;
}

bool CGMap2DTo3DDlg::IsEqual(IGNodePtr pNd1, IGNodePtr pNd2)
{
    if (!pNd1 || !pNd2)
        return false;
    double x1,y1,z1,x2,y2,z2;
    pNd1->get_X(&x1);
    pNd1->get_Y(&y1);
    pNd1->get_Z(&z1);

    pNd2->get_X(&x2);
    pNd2->get_Y(&y2);
    pNd2->get_Z(&z2);

    if (fabs(x1-x2) < ZERO && fabs(y1-y2) < ZERO && fabs(z1-z2) < ZERO)
        return true;
    return false;
}

// 判断 点 是否在有效地转换区域,在则返回true，否则返回false
bool CGMap2DTo3DDlg::PointInValidateRgn(POINT2D3D &ptIn, int &polyID)
{
    int polyid = 0;
    CArray<POINT2D3D,POINT2D3D> poly;
    for (long i = 0; i < m_ctrlpolyIndexArray.GetSize(); i++)
    {
        int index = m_ctrlpolyIndexArray.GetAt(i);
        if (index != -1)
        {
            poly.Add(m_ctrlPointsArray[index]);
        }
        else
        {// -1表示结束了一个poly定义
            long count = poly.GetCount();
            if (count < 1) continue;
            // 如果poly是闭合的，则开始计算，如果没有闭合，程序进行闭合
            if (IsEqual(poly[0].pt2d,poly[count-1].pt2d) == false)
                poly.Add(poly[0]);

            // 一个poly完毕，开始计算
            bool bIn = IsPointInPolygon(poly,ptIn.pt2d);
            if (bIn) // 在多边形内，返回真值
            {
                polyID = polyid;               
                // 二维点转三维，直接存储再ptIn.pt3d中 - 如果poly的点不够3个返回失败
                Point2Dto3D_Ext2(poly,ptIn.pt2d,ptIn.pt3d);
                return true;
            }
            polyid++;
            // 清除上一个poly，继续下一个poly
            poly.RemoveAll();
        }
    }   

    // 在区域外不转换，写入什么值合适???
    polyID = -1;
    return false;
}

void CGMap2DTo3DDlg::AddToSortArray(CArray<POINT2D3D,POINT2D3D> &aryPlanePts,POINT2D3D ptNew)
{// 注: aryPlanPts中的元素是已经根据距离aryPlanPts[0]的二维距离排好序的，我们只要将ptNew添加到合适的位置即可
    int count = aryPlanePts.GetCount();
    if (count <= 0)
    {//  Error : aryPlanePts中必须不为空，它的第一个元素
        //aryPlanePts.Add(ptNew);
        return;
    }   

    double quater_dis = (aryPlanePts[0].pt2d.X - ptNew.pt2d.X)*(aryPlanePts[0].pt2d.X - ptNew.pt2d.X) +
        (aryPlanePts[0].pt2d.Y - ptNew.pt2d.Y) * (aryPlanePts[0].pt2d.Y - ptNew.pt2d.Y);

    double d1,d2;
    for (int i = 1; i < count-1; i++)
    {
        d1 = (aryPlanePts[0].pt2d.X - aryPlanePts[i].pt2d.X)*(aryPlanePts[0].pt2d.X - aryPlanePts[i].pt2d.X) +
            (aryPlanePts[0].pt2d.Y - aryPlanePts[i].pt2d.Y) * (aryPlanePts[0].pt2d.Y - aryPlanePts[i].pt2d.Y);
        d2 = (aryPlanePts[0].pt2d.X - aryPlanePts[i+1].pt2d.X)*(aryPlanePts[0].pt2d.X - aryPlanePts[i+1].pt2d.X) +
            (aryPlanePts[0].pt2d.Y - aryPlanePts[i+1].pt2d.Y) * (aryPlanePts[0].pt2d.Y - aryPlanePts[i+1].pt2d.Y);

        // 如果距离一样，说明点为重复点，不用添加
        if (IsEqual(ptNew.pt2d,aryPlanePts[i].pt2d) == true || IsEqual(ptNew.pt2d,aryPlanePts[i+1].pt2d) == true)
            return;
        if ( (quater_dis-d1)*(quater_dis-d2) < ZERO )
        {// 找到位置，应该将ptNew插入 i 后， i+1前的位置
            aryPlanePts.InsertAt(i+1,ptNew);
            return;
        }
    }

    aryPlanePts.Add(ptNew);
}

 

// 点P(pa,pb,pc)到平面A的垂直距离(平面方程Ax + By + Cz + D = 0)
double PointDistancePlane(double pa,double pb,double pc, double aa,double ab,double ac, double ad)
{       
    double dist = 1;
    double module = Module(aa,ab,ac);
    if (fabs(module) > 0.000001f)
    {
        dist = fabs(aa*pa + ab*pb + ac*pc + ad) /module;
    }
    return dist;
}

 

// 向量P绕任意轴A顺时针旋转theta(弧度值)结果Q
    void  AnyRotate(double &qa, double &qb, double &qc, double pa, double pb, double pc, double aa, double ab, double ac, double theta)
    {
        double temp[3];
        Cross(temp[0],temp[1],temp[2],pa,pb,pc,aa,ab,ac);
        double t = pa * aa + pb * ab + pc * ac;

        qa = pa*cos(theta) + temp[0]*sin(theta) + pa*t*(1-cos(theta));
        qb = pb*cos(theta) + temp[1]*sin(theta) + pb*t*(1-cos(theta));
        qc = pc*cos(theta) + temp[2]*sin(theta) + pc*t*(1-cos(theta));
    }

 

void  normalize(double &x, double &y, double &z)
{
    double module = sqrt(x*x + y*y + z*z);

    if (fabs(module) > 0.000001f)
    {
        x /= module;
        y /= module;
        z /= module;
    }
}

double Module(double x, double y, double z)
{
    return sqrt(x*x + y*y + z*z);
}

double Dot( double x1,double y1,double z1, double x2,double y2, double z2)
{
    return (x1*x2 + y1*y2 + z1*z2);
}

void Cross(double &x, double &y, double &z, double x1,double y1,double z1, double x2,double y2, double z2)
{
    x = y1 * z2 - z1 * y2;
    y = z1 * x2 - x1 * z2;
    z = x1 * y2 - y1 * x2;
}

 

// 判断点是否在多边形内(包括凹多边形和凸多边形) - 二维平面
// 定义判断，从点向水平(或垂直方向做延长线)计算交点，看在点右边的交点数目，奇数表示在poly内，偶数表示在poly外
bool CGMap2DTo3DDlg::IsPointInPolygon(CArray<POINT2D3D,POINT2D3D> &poly,POINT2D pt2DIn)
{
#if 1 // 射线算法 - 适用于凹凸多边形
    long count=0;
    //float sub_count=0;
    for (long i = 0; i < poly.GetCount()-1; i++)
    {
        POINT2D Ln2_pt1 = poly.GetAt(i).pt2d;
        POINT2D Ln2_pt2 = poly.GetAt(i+1).pt2d;       
        if (Ln2_pt1.Y == Ln2_pt2.Y)
            continue;
        if ( (pt2DIn.Y < Ln2_pt1.Y) && (pt2DIn.Y < Ln2_pt2.Y))// 交点在p1p2延长线上
            continue;
        if ( (pt2DIn.Y >= Ln2_pt1.Y) && (pt2DIn.Y >= Ln2_pt2.Y))// 交点在p1p2延长线上
            continue;
        // 求交点的 X 坐标 --------------------------------------------------------------
        double x = (pt2DIn.Y - Ln2_pt1.Y) * (Ln2_pt2.X - Ln2_pt1.X) / (Ln2_pt2.Y - Ln2_pt1.Y) + Ln2_pt1.X;

        if ( x > pt2DIn.X )
            count++; // 只统计单边交点    
    }
    if (count%2==0)//有进有出，表示在点ptInter在poly外
        return false;
#else// 叉乘算法只适用于凸多边形
    double res;
    bool bset = false;
    for (long i = 0; i < poly.GetCount()-1; i++)
    {
        POINT2D Ln2_pt1 = poly.GetAt(i).pt2d;
        POINT2D Ln2_pt2 = poly.GetAt(i+1).pt2d;

        double res2 = (Ln2_pt2.X-Ln2_pt1.X) * (pt2DIn.Y-Ln2_pt1.Y) - (Ln2_pt2.Y-Ln2_pt1.Y)*(pt2DIn.X-Ln2_pt1.X);
        if (res2 == 0)
        {
            if ( (pt2DIn.X-Ln2_pt1.X)*(pt2DIn.X-Ln2_pt2.X) <= ZERO && (pt2DIn.Y-Ln2_pt1.Y)*(pt2DIn.Y-Ln2_pt2.Y) <= ZERO )
                return true;           
        }
        else
        {
            if (!bset)
            {
                res = res2;
                bset = true;
            }
            else
            {
                if (res*res2 < 0)// 不在面内
                    return false;
            }           
        }
    }
#endif
    return true;
}