C++：角度，距离计算，点面距离，点投影到面

---

角度计算

角度转弧度

double d2rad(double d, double m, double s)//角度转弧度
{
	double e;
	double sign = (d<0.0) ? -1.0 : 1.0;
	if (d == 0)
	{
		sign = (m<0.0) ? -1.0 : 1.0;
		if (m == 0)
		{
			sign = (s<0.0) ? -1.0 : 1.0;
		}
	}
	if (d<0)
		d = d*(-1.0);
	if (m<0)
		m = m*(-1.0);
	if (s<0)
		s = s*(-1.0);

	//a为整数度b为分c为秒
	e = sign*(d * 3600 + m * 60 + s)*PI / (3600 * 180);
	return e;
}

//该函数为弧度化为度分秒形式的角度，
//如0.029088820866572159615394846141477的弧度化为度分秒的角度值为1.4000
double Rad2Ddmmss(double rad)
{
    int f = rad >= 0 ? 1 : -1; // 符号＋ －
    //加0.001秒（用弧度表示），化为度
    rad = (rad + f * 4.8481368110953599358991410235795e-9) * PI/180;
    int d = (int)rad;//将弧度换算成以度为单位
    rad = (rad - d) * 60.0;
    int m = (int)rad;
    double s = (rad - m) * 60.0;

    return d + m / 100.0 + s / 10000.0 - f * 0.0000001;
}

距离计算

点到面的距离

求P0N

double ComputeDistancePointToPlane(double pt[3], double dPlanMinPoint[3], double dPlanMaxPoint[3])
{
	//Todo：计算点和平面的距离
	double X = pt[0];//与XOY平面垂直
	double Y = pt[1];
	double Z = pt[2];
	double p1[3] = { dPlanMinPoint[0], dPlanMinPoint[1], dPlanMaxPoint[2] };
	double p2[3] = { dPlanMaxPoint[0], dPlanMaxPoint[1], dPlanMaxPoint[2] };
	double p3[3] = { dPlanMinPoint[0], dPlanMinPoint[1], dPlanMinPoint[2] };
	double x1 = p1[0];
	double y1 = p1[1];
	double z1 = p1[2];
	double x2 = p2[0];
	double y2 = p2[1];
	double z2 = p2[2];
	double x3 = p3[0];
	double y3 = p3[1];
	double z3 = p3[2];
	//平面一般式方程：Ax+By+Cz=D
	double A = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
	double B = (x3 - x1) * (z2 - z1) - (x2 - x1) * (z3 - z1);
	double C = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
	double D = -(A * x1 + B * y1 + C * z1);
	double distance = fabs(A * X + B * Y + C * Z + D) / sqrt(A * A + B * B + C * C);
	return distance;
}

点投影到面，求投影点

求点N的三维坐标

void ProjectPointToPlane(double pt[3], double dPlanMinPoint[3], double dPlanMaxPoint[3], double ptReturn[3]) {
	//Todo：计算点投影到平面，返回投影点N
	double x0 = pt[0];//与XOY平面垂直
	double y0 = pt[1];
	double z0 = pt[2];
	double p1[3] = { dPlanMinPoint[0], dPlanMinPoint[1], dPlanMaxPoint[2] };
	double p2[3] = { dPlanMaxPoint[0], dPlanMaxPoint[1], dPlanMaxPoint[2] };
	double p3[3] = { dPlanMinPoint[0], dPlanMinPoint[1], dPlanMinPoint[2] };
	double x1 = p1[0];
	double y1 = p1[1];
	double z1 = p1[2];
	double x2 = p2[0];
	double y2 = p2[1];
	double z2 = p2[2];
	double x3 = p3[0];
	double y3 = p3[1];
	double z3 = p3[2];
	//平面一般式方程：Ax+By+Cz=D
	double A = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
	double B = (x3 - x1) * (z2 - z1) - (x2 - x1) * (z3 - z1);
	double C = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
	double D = -(A * x1 + B * y1 + C * z1);
	double t = (A * x0 + B * y0 + C * z0 + D) / (A * A + B * B + C * C);
	ptReturn[0] = x0 - A * t;
	ptReturn[1] = y0 - B * t;
	ptReturn[2] = z0 - C * t;
}

点到点

点到线