// 二维点转三维函数[核心]
// 输入参数 : 转换平面上的点数组,二维点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;
}