double GetDistanceToLine(const double ptStartX,

const double ptStartY,

const double ptStartZ,

const double ptEndX,

const double ptEndY,

const double ptEndZ,

const double ptX,

const double ptY,

const double ptZ,

double& ptPedalX,

double& ptPedalY,

double& ptPedalZ)

{

if (IsZero(GetDistance(ptStartX, ptStartY, ptStartZ, ptEndX,

ptEndY, ptEndZ)))

{

return -1;

}

double d = 0; // 系数，因为垂足在直线上，所以，垂足可以用起点+方向向量X一个系数表示

double vtDirectionX = ptEndX -

ptStartX;

double vtDirectionY = ptEndY - ptStartY;

double vtDirectionZ = ptEndZ - ptStartZ;

// 一定要单位化，下面求d时就不需要分母了

double dLength = sqrt(vtDirectionX * vtDirectionX +

vtDirectionY * vtDirectionY + vtDirectionZ * vtDirectionZ);

vtDirectionX = vtDirectionX / dLength;

vtDirectionY = vtDirectionY / dLength;

vtDirectionZ = vtDirectionZ / dLength;

垂足与pt的向量

//double x1 = ptStartX + vtDirectionX * d - ptX;

//double y1 = ptStartY + vtDirectionY * d - ptY;

//double z1 = ptStartZ + vtDirectionZ * d - ptZ;

直线的向量

//double x2 = vtDirectionX;

//double y2 = vtDirectionY;

//double z2 = vtDirectionZ;

// vt1 * vt2 = 0导出d

// x1 * x2 + y1 * y2 + z1 * z2 = 0;

d = ptX * vtDirectionX - ptStartX * vtDirectionX +

ptY * vtDirectionY - ptStartY * vtDirectionY +

ptZ * vtDirectionZ - ptStartZ * vtDirectionZ;

// 求出垂足

ptPedalX = ptStartX + vtDirectionX * d;

ptPedalY = ptStartY + vtDirectionY * d;

ptPedalZ = ptStartZ + vtDirectionZ * d;

return GetDistance(ptPedalX, ptPedalY, ptPedalZ, ptX, ptY,

ptZ);

}

// 两点的距离

double GetDistance( const double ptStartX, const double

ptStartY, const double ptStartZ, const double ptEndX, const double

ptEndY, const double ptEndZ )

{

double x = ptStartX - ptEndX;

double y = ptStartY - ptEndY;

double z = ptStartZ - ptEndZ;

return sqrt(x * x + y * y + z * z);

}

// 判断给定的double值是否为0

bool IsZero( double a, double tol )

{

return fabs(a) < tol;

}