opc_lsstrioverlap.h

opcode是功能强大

// Following code from Magic-Software (http://www.magic-software.com/)
// A bit modified for Opcode

static const float gs_fTolerance = 1e-05f;

static float OPC_PointTriangleSqrDist(const IceMaths::Point& point, const IceMaths::Point& p0, const IceMaths::Point& p1, const IceMaths::Point& p2)
{
	// Hook
	IceMaths::Point TriEdge0 = p1 - p0;
	IceMaths::Point TriEdge1 = p2 - p0;

	IceMaths::Point kDiff	= p0 - point;
	float fA00	= TriEdge0.SquareMagnitude();
	float fA01	= TriEdge0 | TriEdge1;
	float fA11	= TriEdge1.SquareMagnitude();
	float fB0	= kDiff | TriEdge0;
	float fB1	= kDiff | TriEdge1;
	float fC	= kDiff.SquareMagnitude();
	float fDet	= fabsf(fA00*fA11 - fA01*fA01);
	float fS	= fA01*fB1-fA11*fB0;
	float fT	= fA01*fB0-fA00*fB1;
	float fSqrDist;

	if(fS + fT <= fDet)
	{
		if(fS < 0.0f)
		{
			if(fT < 0.0f)  // region 4
			{
				if(fB0 < 0.0f)
				{
					if(-fB0 >= fA00)		fSqrDist = fA00+2.0f*fB0+fC;
					else					fSqrDist = fB0*(-fB0/fA00)+fC;
				}
				else
				{
					if(fB1 >= 0.0f)			fSqrDist = fC;
					else if(-fB1 >= fA11)	fSqrDist = fA11+2.0f*fB1+fC;
					else					fSqrDist = fB1*(-fB1/fA11)+fC;
				}
			}
			else  // region 3
			{
				if(fB1 >= 0.0f)				fSqrDist = fC;
				else if(-fB1 >= fA11)		fSqrDist = fA11+2.0f*fB1+fC;
				else						fSqrDist = fB1*(-fB1/fA11)+fC;
			}
		}
		else if(fT < 0.0f)  // region 5
		{
			if(fB0 >= 0.0f)					fSqrDist = fC;
			else if(-fB0 >= fA00)			fSqrDist = fA00+2.0f*fB0+fC;
			else							fSqrDist = fB0*(-fB0/fA00)+fC;
		}
		else  // region 0
		{
			// minimum at interior point
			if(fDet==0.0f)
			{
				fSqrDist = MAX_FLOAT;
			}
			else
			{
				float fInvDet = 1.0f/fDet;
				fS *= fInvDet;
				fT *= fInvDet;
				fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
			}
		}
	}
	else
	{
		float fTmp0, fTmp1, fNumer, fDenom;

		if(fS < 0.0f)  // region 2
		{
			fTmp0 = fA01 + fB0;
			fTmp1 = fA11 + fB1;
			if(fTmp1 > fTmp0)
			{
				fNumer = fTmp1 - fTmp0;
				fDenom = fA00-2.0f*fA01+fA11;
				if(fNumer >= fDenom)
				{
					fSqrDist = fA00+2.0f*fB0+fC;
				}
				else
				{
					fS = fNumer/fDenom;
					fT = 1.0f - fS;
					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
				}
			}
			else
			{
				if(fTmp1 <= 0.0f)		fSqrDist = fA11+2.0f*fB1+fC;
				else if(fB1 >= 0.0f)	fSqrDist = fC;
				else					fSqrDist = fB1*(-fB1/fA11)+fC;
			}
		}
		else if(fT < 0.0f)  // region 6
		{
			fTmp0 = fA01 + fB1;
			fTmp1 = fA00 + fB0;
			if(fTmp1 > fTmp0)
			{
				fNumer = fTmp1 - fTmp0;
				fDenom = fA00-2.0f*fA01+fA11;
				if(fNumer >= fDenom)
				{
					fSqrDist = fA11+2.0f*fB1+fC;
				}
				else
				{
					fT = fNumer/fDenom;
					fS = 1.0f - fT;
					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
				}
			}
			else
			{
				if(fTmp1 <= 0.0f)		fSqrDist = fA00+2.0f*fB0+fC;
				else if(fB0 >= 0.0f)	fSqrDist = fC;
				else					fSqrDist = fB0*(-fB0/fA00)+fC;
			}
		}
		else  // region 1
		{
			fNumer = fA11 + fB1 - fA01 - fB0;
			if(fNumer <= 0.0f)
			{
				fSqrDist = fA11+2.0f*fB1+fC;
			}
			else
			{
				fDenom = fA00-2.0f*fA01+fA11;
				if(fNumer >= fDenom)
				{
					fSqrDist = fA00+2.0f*fB0+fC;
				}
				else
				{
					fS = fNumer/fDenom;
					fT = 1.0f - fS;
					fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
				}
			}
		}
	}
	return fabsf(fSqrDist);
}

static float OPC_SegmentSegmentSqrDist(const IceMaths::Segment& rkSeg0, const IceMaths::Segment& rkSeg1)
{
	// Hook
	IceMaths::Point rkSeg0Direction	= rkSeg0.ComputeDirection();
	IceMaths::Point rkSeg1Direction	= rkSeg1.ComputeDirection();

	IceMaths::Point kDiff	= rkSeg0.mP0 - rkSeg1.mP0;
	float fA00	= rkSeg0Direction.SquareMagnitude();
	float fA01	= -rkSeg0Direction.Dot(rkSeg1Direction);
	float fA11	= rkSeg1Direction.SquareMagnitude();
	float fB0	= kDiff.Dot(rkSeg0Direction);
	float fC	= kDiff.SquareMagnitude();
	float fDet	= fabsf(fA00*fA11-fA01*fA01);

	float fB1, fS, fT, fSqrDist, fTmp;

	if(fDet>=gs_fTolerance)
	{
		// line segments are not parallel
		fB1 = -kDiff.Dot(rkSeg1Direction);
		fS = fA01*fB1-fA11*fB0;
		fT = fA01*fB0-fA00*fB1;

		if(fS >= 0.0f)
		{
			if(fS <= fDet)
			{
				if(fT >= 0.0f)
				{
					if(fT <= fDet)  // region 0 (interior)
					{
						// minimum at two interior points of 3D lines
						float fInvDet = 1.0f/fDet;
						fS *= fInvDet;
						fT *= fInvDet;
						fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
					}
					else  // region 3 (side)
					{
						fTmp = fA01+fB0;
						if(fTmp>=0.0f)			fSqrDist = fA11+2.0f*fB1+fC;
						else if(-fTmp>=fA00)	fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
						else					fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
					}
				}
				else  // region 7 (side)
				{
					if(fB0>=0.0f)				fSqrDist = fC;
					else if(-fB0>=fA00)			fSqrDist = fA00+2.0f*fB0+fC;
					else						fSqrDist = fB0*(-fB0/fA00)+fC;
				}
			}
			else
			{
				if ( fT >= 0.0 )
				{
					if ( fT <= fDet )  // region 1 (side)
					{
						fTmp = fA01+fB1;
						if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
						else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
						else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
					}
					else  // region 2 (corner)
					{
						fTmp = fA01+fB0;
						if ( -fTmp <= fA00 )
						{
							if(fTmp>=0.0f)		fSqrDist = fA11+2.0f*fB1+fC;
							else				fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
						}
						else
						{
							fTmp = fA01+fB1;
							if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
							else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
							else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
						}
					}
				}
				else  // region 8 (corner)
				{
					if ( -fB0 < fA00 )
					{
						if(fB0>=0.0f)	fSqrDist = fC;
						else			fSqrDist = fB0*(-fB0/fA00)+fC;
					}
					else
					{
						fTmp = fA01+fB1;
						if(fTmp>=0.0f)			fSqrDist = fA00+2.0f*fB0+fC;
						else if(-fTmp>=fA11)	fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
						else					fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
					}
				}
			}
		}
		else 
		{
			if ( fT >= 0.0f )
			{
				if ( fT <= fDet )  // region 5 (side)
				{
					if(fB1>=0.0f)		fSqrDist = fC;
					else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
					else				fSqrDist = fB1*(-fB1/fA11)+fC;
				}
				else  // region 4 (corner)
				{
					fTmp = fA01+fB0;
					if ( fTmp < 0.0f )
					{
						if(-fTmp>=fA00)	fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
						else			fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
					}
					else
					{
						if(fB1>=0.0f)		fSqrDist = fC;
						else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
						else				fSqrDist = fB1*(-fB1/fA11)+fC;
					}
				}
			}
			else   // region 6 (corner)
			{
				if ( fB0 < 0.0f )
				{
					if(-fB0>=fA00)	fSqrDist = fA00+2.0f*fB0+fC;
					else			fSqrDist = fB0*(-fB0/fA00)+fC;
				}
				else
				{
					if(fB1>=0.0f)		fSqrDist = fC;
					else if(-fB1>=fA11)	fSqrDist = fA11+2.0f*fB1+fC;
					else				fSqrDist = fB1*(-fB1/fA11)+fC;
				}
			}
		}
	}
	else
	{
		// line segments are parallel
		if ( fA01 > 0.0f )
		{
			// direction vectors form an obtuse angle
			if ( fB0 >= 0.0f )
			{
				fSqrDist = fC;
			}
			else if ( -fB0 <= fA00 )
			{
				fSqrDist = fB0*(-fB0/fA00)+fC;
			}
			else
			{
				fB1 = -kDiff.Dot(rkSeg1Direction);
				fTmp = fA00+fB0;
				if ( -fTmp >= fA01 )
				{
					fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
				}
				else
				{
					fT = -fTmp/fA01;
					fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
				}
			}
		}
		else
		{
			// direction vectors form an acute angle
			if ( -fB0 >= fA00 )
			{
				fSqrDist = fA00+2.0f*fB0+fC;
			}
			else if ( fB0 <= 0.0f )
			{
				fSqrDist = fB0*(-fB0/fA00)+fC;
			}
			else
			{
				fB1 = -kDiff.Dot(rkSeg1Direction);
				if ( fB0 >= -fA01 )
				{
					fSqrDist = fA11+2.0f*fB1+fC;
				}
				else
				{
					fT = -fB0/fA01;
					fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);