数学-几何

点到直线的垂足

Point1 GetFootOfPerpendicular(const Point1 &pt, const Point1 &begin, const Point1 &end)
{
	Point1 retVal;

	double dx = begin.x - end.x;
	double dy = begin.y - end.y;
	double dz = begin.z - end.z;
	if (abs(dx) < 0.00000001 && abs(dy) < 0.00000001 && abs(dz) < 0.00000001)
	{
		retVal = begin;
		return retVal;
	}

	double u = (pt.x - begin.x)*(begin.x - end.x) +
		(pt.y - begin.y)*(begin.y - end.y) + (pt.z - begin.z)*(begin.z - end.z);
	u = u / ((dx*dx) + (dy*dy) + (dz*dz));

	retVal.x = begin.x + u*dx;
	retVal.y = begin.y + u*dy;
	retVal.z = begin.z + u*dz;

	return retVal;
}

直线与圆的交点

int CompNode(double * v, double r, double *node, double c, double d)
{
	double k, b;                                  //直线方程：y = kx + b；   //圆方程：(x - c) ^ 2 + (y - d) ^ 2 = r ^ 2;       

	k = (v[1] - v[3]) / (v[0] - v[2]);
	b = v[1] - k*v[0];

	if (fabs(k*c - d + b) / sqrt(k*k + b*b))
	{
		node[0] = (2 * c - 2 * k*(b - d) + sqrt(pow((2 * k*(b - d) - 2 * c), 2) - 4 * (k*k + 1)*((b - d)*(b - d) + c*c - r*r))) / (2 * k*k + 2);
		node[1] = k*node[0] + b;
		node[2] = (2 * c - 2 * k*(b - d) - sqrt(pow((2 * k*(b - d) - 2 * c), 2) - 4 * (k*k + 1)*((b - d)*(b - d) + c*c - r*r))) / (2 * k*k + 2);
		node[3] = k*node[2] + b;
	}

	return 0;
}

直线与球的交点

int FindLineSphereIntersections(Point1 linePoint0, Point1 linePoint1, Point1 circleCenter, double circleRadius, Point1 *node)
{

	double cx = circleCenter.x;
	double cy = circleCenter.y;
	double cz = circleCenter.z;

	double px = linePoint0.x;
	double py = linePoint0.y;
	double pz = linePoint0.z;

	double vx = linePoint1.x - px;
	double vy = linePoint1.y - py;
	double vz = linePoint1.z - pz;

	double A = vx * vx + vy * vy + vz * vz;
	double B = 2.0 * (px * vx + py * vy + pz * vz - vx * cx - vy * cy - vz * cz);
	double C = px * px - 2 * px * cx + cx * cx + py * py - 2 * py * cy + cy * cy +
		pz * pz - 2 * pz * cz + cz * cz - circleRadius * circleRadius;

	// discriminant
	double D = B * B - 4 * A * C;

	if (D < 0)
	{
		return -1;
	}

	double t1 = (-B - sqrt(D)) / (2.0 * A);

	node[0].x = linePoint0.x * (1 - t1) + t1 * linePoint1.x;
	node[0].y = linePoint0.y * (1 - t1) + t1 * linePoint1.y;
	node[0].z = linePoint0.z * (1 - t1) + t1 * linePoint1.z;

	if (D == 0)
	{
		return 0;
	}

	double t2 = (-B + sqrt(D)) / (2.0 * A);
	node[1].x = linePoint0.x * (1 - t2) + t2 * linePoint1.x;
	node[1].y = linePoint0.y * (1 - t2) + t2 * linePoint1.y;
	node[1].z = linePoint0.z * (1 - t2) + t2 * linePoint1.z;


	// prefer a solution that's on the line segment itself

	if (abs(t1 - 0.5) < abs(t2 - 0.5))
	{
		return 0;
	}

	return 0;
}