【计算几何】求三角面和直线交点

需求：

对于给定的三角形面片3个顶点，和一条直线的2个点，求三角面和直线的交点，若无交点，输出-1。

思路：

利用海伦公式，可以得到三角形的面积，然后用3个点的2个向量，进行叉乘，得到面的法向量。ax+by+cz=d可以表示面，求出常数d，联力面的方程和直线方程，求解交点。

代码：

class CVector
{
public:
	union
	{
		float vec[3];
		struct { float x, y, z; };
	};
};

class CrossPoint
{
public:
	CrossPoint();
	virtual ~CrossPoint();
public:
	static bool ValidPoint(CVector &LinePoint, CVector &LineV,
		CVector &TrianglePoint1, CVector &TrianglePoint2, CVector &TrianglePoint3, CVector &result);
	static float Area(float a, float b, float c);
	static float Distance(CVector &p1, CVector &p2);
};

///////////////////////////////

CrossPoint::CrossPoint()
{

}

CrossPoint::~CrossPoint()
{

}
//计算p1到p2的距离的平方
float CrossPoint::Distance(CVector &p1, CVector &p2)
{
	float dist;
	dist = ((p2.x - p1.x)*(p2.x - p1.x)
		+ (p2.y - p1.y)*(p2.y - p1.y)
		+ (p2.z - p1.z)*(p2.z - p1.z));
	return (float)sqrt(dist);
}
//利用海伦公式求变成为a,b,c的三角形的面积
float CrossPoint::Area(float a, float b, float c)
{
	float s = (a + b + c) / 2;
	return (float)sqrt(s*(s - a)*(s - b)*(s - c));
}


bool CrossPoint::ValidPoint(CVector &LinePoint1, CVector &LinePoint2, CVector &TrianglePoint1, CVector

	&TrianglePoint2, CVector &TrianglePoint3, CVector &result)
{
	//三角形所在平面的法向量
	CVector TriangleV;
	//三角形的边方向向量
	CVector VP12, VP13;
	//直线与平面的交点
	CVector CrossPoint;
	//平面方程常数项
	float TriD;
	//CVector LineV = LinePoint2 - LinePoint1;
	CVector LineV;
	LineV.x = 0, LineV.y = 0, LineV.z = 100;
	/*-------计算平面的法向量及常数项-------*/
	//point1->point2
	VP12.x = TrianglePoint2.x - TrianglePoint1.x;
	VP12.y = TrianglePoint2.y - TrianglePoint1.y;
	VP12.z = TrianglePoint2.z - TrianglePoint1.z;
	//point1->point3
	VP13.x = TrianglePoint3.x - TrianglePoint1.x;
	VP13.y = TrianglePoint3.y - TrianglePoint1.y;
	VP13.z = TrianglePoint3.z - TrianglePoint1.z;
	//VP12xVP13
	TriangleV.x = VP12.y*VP13.z - VP12.z*VP13.y;
	TriangleV.y = -(VP12.x*VP13.z - VP12.z*VP13.x);
	TriangleV.z = VP12.x*VP13.y - VP12.y*VP13.x;
	//计算常数项
	TriD = -(TriangleV.x*TrianglePoint1.x
		+ TriangleV.y*TrianglePoint1.y
		+ TriangleV.z*TrianglePoint1.z);
	/*-------求解直线与平面的交点坐标---------*/
	/* 思路：
	* 首先将直线方程转换为参数方程形式，然后代入平面方程，求得参数t，
	* 将t代入直线的参数方程即可求出交点坐标
	*/
	float tempU, tempD; //临时变量
	tempU = TriangleV.x*LinePoint1.x + TriangleV.y*LinePoint1.y
		+ TriangleV.z*LinePoint1.z + TriD;
	tempD = TriangleV.x*LineV.x + TriangleV.y*LineV.y + TriangleV.z*LineV.z;
	//直线与平面平行或在平面上
	if (tempD == 0.0)
	{
		//printf("The line is parallel with the plane.
");
		return false;
	}
	//计算参数t
	float t = -tempU / tempD;
	//计算交点坐标
	CrossPoint.x = LineV.x*t + LinePoint1.x;
	CrossPoint.y = LineV.y*t + LinePoint1.y;
	CrossPoint.z = LineV.z*t + LinePoint1.z;
	/*----------判断交点是否在三角形内部---*/

	//计算三角形三条边的长度
	float d12 = Distance(TrianglePoint1, TrianglePoint2);
	float d13 = Distance(TrianglePoint1, TrianglePoint3);
	float d23 = Distance(TrianglePoint2, TrianglePoint3);
	//计算交点到三个顶点的长度
	float c1 = Distance(CrossPoint, TrianglePoint1);
	float c2 = Distance(CrossPoint, TrianglePoint2);
	float c3 = Distance(CrossPoint, TrianglePoint3);
	//求三角形及子三角形的面积
	float areaD = Area(d12, d13, d23); //三角形面积
	float area1 = Area(c1, c2, d12); //子三角形1
	float area2 = Area(c1, c3, d13); //子三角形2
	float area3 = Area(c2, c3, d23); //子三角形3
									 //根据面积判断点是否在三角形内部
	if (fabs(area1 + area2 + area3 - areaD) > 0.001)
	{
		return false;
	}

	result = CrossPoint;
	return true;
}