异面直线公垂线的垂足坐标计算公式推导(二)
根据异面直线公垂线的垂足坐标计算公式，定义逻辑类，计算垂足的坐标值。用C#定义该逻辑类，如下
/// <summary>
///公垂线垂足坐标通用推导公式
/// </summary>
public class CommonPerpendicular
{
//AB构成的直线
private IPoint ptA = null;
private IPoint ptB = null;
//AB直线上的垂足
private IPoint ptM = null;
//CD构成的直线
private IPoint ptC = null;
private IPoint ptD = null;
//CD直线上的垂足
private IPoint ptN = null;
public CommonPerpendicular()
{
try
{
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("PipeProjFactory:" + ex.Message);
}
}
/// <summary>
/// 计算异面直线的公垂线两个垂足的坐标
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <param name="C"></param>
/// <param name="D"></param>
/// <param name="M">AB直线上的垂足坐标M</param>
/// <param name="N">CD直线上的垂足坐标N</param>
/// <returns></returns>
public bool ComputePerpendicular(IPoint A, IPoint B, IPoint C, IPoint D, IPoint M, IPoint N)
{
try
{
if (null == A || null == B || null == C || null == D || null == M || null == N)
{
return false;
}
ptA = A;
ptB = B;
ptC = C;
ptD = D;
ptM = M;
ptN = N;
//f1ab
double fSquareAB = FunSquare(ptA, ptB);
if (-99999999.99999 == fSquareAB)
{
return false;
}
//f1cd
double fSquareCD = FunSquare(ptC, ptD);
if (-99999999.99999 == fSquareCD)
{
return false;
}
//f3ab
double fVaryAB = FunVaryOperation(ptA, ptB);
if (-99999999.99999 == fVaryAB)
{
return false;
}
//f3cd
double fVaryCD = FunVaryOperation(ptC, ptD);
if (-99999999.99999 == fVaryCD)
{
return false;
}
//f2
double fCross = FunCrossOperation();
if (-99999999.99999 == fCross)
{
return false;
}
double t1 = (fVaryAB * fSquareCD - fVaryCD * fCross) / (fSquareAB * fSquareCD - fCross * fCross);
double t2 = (fVaryCD * fSquareAB - fCross * fVaryAB) / (fCross * fCross - fSquareAB * fSquareCD);
ptM.X = t1 * (ptB.X - ptA.X) + ptA.X;
ptM.Y = t1 * (ptB.Y - ptA.Y) + ptA.Y;
ptM.Z = t1 * (ptB.Z - ptA.Z) + ptA.Z;
ptN.X = t2 * (ptD.X - ptC.X) + ptC.X;
ptN.Y = t2 * (ptD.Y - ptC.Y) + ptC.Y;
ptN.Z = t2 * (ptD.Z - ptC.Z) + ptC.Z;
return true;
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("PipeProjFactory:" + ex.Message);
return false;
}
}
/// <summary>
/// [(xb-xa)*(xb-xa)+(yb-ya)*(yb-ya)+(zb-za)*(zb-za)]，两个点可以被替换
/// </summary>
/// <param name="P1"></param>
/// <param name="P2"></param>
/// <returns></returns>
private double FunSquare(IPoint P1, IPoint P2)
{
try
{
double funValue = 0.0;
funValue = Math.Sqrt(P2.X - P1.X) + Math.Sqrt(P2.Y - P1.Y) + Math.Sqrt(P2.Z - P1.Z);
return funValue;
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("PipeProjFactory:" + ex.Message);
return -99999999.99999;
}
}
/// <summary>
/// (xb-xa)*(xc-xa)+(yb-ya)*(yc-ya)+(zb-za)*(zc-za),每个和项的第一个公式可以被替换
/// </summary>
/// <param name="P1"></param>
/// <param name="P2"></param>
/// <returns></returns>
private double FunVaryOperation(IPoint P1,IPoint P2)
{
try
{
double funValue = 0.0;
funValue = (P2.X-P1.X)*(ptC.X -ptA.X)+(P2.Y-P1.Y)*(ptC.Y-ptA.Y)+(P2.Z-P1.Z)*(ptC.Z-ptA.Z);
return funValue;
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("PipeProjFactory:" + ex.Message);
return -99999999.99999;
}
}
/// <summary>
/// (xb-xa)*(xd-xc)+(yb-ya)*(yd-yc)+(zb-za)*(zd-zc)
/// </summary>
/// <returns></returns>
private double FunCrossOperation()
{
try
{
double funValue = 0.0;
funValue = (ptB.X - ptA.X) * (ptD.X - ptC.X) + (ptB.Y - ptA.Y) * (ptD.Y - ptC.Y) + (ptB.Z - ptA.Z) * (ptD.Z - ptC.Z);
return funValue;
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("PipeProjFactory:" + ex.Message);
return -99999999.99999;
}
}
}
在逻辑函数定义中，已知P1和P2定义的直线，Q1和Q2定义的直线。判断这两个异面直线的公垂线垂足是否在各自线段范围内,即垂足PPNormalPoint是否在P1P2线段范围内，垂足QQNormalPoint是否在Q1Q2线段范围内。
/// <summary>
/// 计算公垂线的两个垂足，判断是否都在4个点确定的两条线段范围上
/// </summary>
/// <returns></returns>
private bool GetNormalCoplanarPoints()
{
try
{
IPoint PPNormalPoint = new PointClass();
IPoint QQNormalPoint = new PointClass();
CommonPerpendicular commonPerPoint = new CommonPerpendicular();
if (commonPerPoint.ComputePerpendicular(P1, P2, Q1, Q2, PPNormalPoint, QQNormalPoint))
{
if (IsPointInLine(PPNormalPoint, P1, P2) && IsPointInLine(QQNormalPoint, Q1, Q2))
{
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("CollisionAnalysis:" + ex.Message);
return false;
}
}
/// <summary>
/// 直线上一点是否在起终点确定线段上
/// </summary>
/// <param name="pNormal"></param>
/// <param name="pStart"></param>
/// <param name="pEnd"></param>
/// <returns></returns>
private bool IsPointInLine(IPoint pNormal, IPoint pStart, IPoint pEnd)
{
try
{
if (null == pNormal || null == pStart || null == pEnd)
{
return false;
}
double x = pNormal.X;
double y = pNormal.Y;
double z = pNormal.Z;
double minx = pStart.X > pEnd.X ? pEnd.X : pStart.X;
double maxx = pStart.X > pEnd.X ? pStart.X : pEnd.X;
double miny = pStart.Y > pEnd.Y ? pEnd.Y : pStart.Y;
double maxy = pStart.Y > pEnd.Y ? pStart.Y : pEnd.Y;
double minz = pStart.Z > pEnd.Z ? pEnd.Z : pStart.Z;
double maxz = pStart.Z > pEnd.Z ? pStart.Z : pEnd.Z;
if ((x > minx && x < maxx) && (y > miny && y < maxy) && (z > minz && z < maxz))
{
return true;
}
else
{
return false;
}
}
catch (Exception ex)
{
if (AppLog.log.IsErrorEnabled)
AppLog.log.Error("CollisionAnalysis:" + ex.Message);
return false;
}
}
