Real DistLine3Rectangle3<Real>::GetSquared ()
{
// Test if line intersects rectangle. If so, the squared distance is
// zero.
Vector3<Real> N = mRectangle->Axis[0].Cross( mRectangle->Axis[1] );
Real NdD = N.Dot( mLine->Direction );
if ( Math<Real>::FAbs( NdD ) > Math<Real>::ZERO_TOLERANCE )
{
// The line and rectangle are not parallel, so the line intersects
// the plane of the rectangle.
Vector3<Real> diff = mLine->Origin - mRectangle->Center;
Vector3<Real> U, V;
Vector3<Real>::GenerateComplementBasis( U, V, mLine->Direction );
Real UdD0 = U.Dot( mRectangle->Axis[0] );
Real UdD1 = U.Dot( mRectangle->Axis[1] );
Real UdPmC = U.Dot( diff );
Real VdD0 = V.Dot( mRectangle->Axis[0] );
Real VdD1 = V.Dot( mRectangle->Axis[1] );
Real VdPmC = V.Dot( diff );
Real invDet = ( ( Real )1 ) / ( UdD0 * VdD1 - UdD1 * VdD0 );
// Rectangle coordinates for the point of intersection.
Real s0 = ( VdD1 * UdPmC - UdD1 * VdPmC ) * invDet;
Real s1 = ( UdD0 * VdPmC - VdD0 * UdPmC ) * invDet;
if ( Math<Real>::FAbs( s0 ) <= mRectangle->Extent[0]
&& Math<Real>::FAbs( s1 ) <= mRectangle->Extent[1] )
{
// Line parameter for the point of intersection.
Real DdD0 = mLine->Direction.Dot( mRectangle->Axis[0] );
Real DdD1 = mLine->Direction.Dot( mRectangle->Axis[1] );
Real DdDiff = mLine->Direction.Dot( diff );
mLineParameter = s0 * DdD0 + s1 * DdD1 - DdDiff;
// Rectangle coordinates for the point of intersection.
mRectCoord[0] = s0;
mRectCoord[1] = s1;
// The intersection point is inside or on the rectangle.
mClosestPoint0 = mLine->Origin +
mLineParameter * mLine->Direction;
mClosestPoint1 = mRectangle->Center +
s0 * mRectangle->Axis[0] + s1 * mRectangle->Axis[1];
return ( Real )0;
}
}
// Either (1) the line is not parallel to the rectangle and the point of
// intersection of the line and the plane of the rectangle is outside the
// rectangle or (2) the line and rectangle are parallel. Regardless, the
// closest point on the rectangle is on an edge of the rectangle. Compare
// the line to all four edges of the rectangle.
Real sqrDist = Math<Real>::MAX_REAL;
Vector3<Real> scaledDir[2] =
{
mRectangle->Extent[0]* mRectangle->Axis[0],
mRectangle->Extent[1]* mRectangle->Axis[1]
};
for ( int i1 = 0; i1 < 2; ++i1 )
{
for ( int i0 = 0; i0 < 2; ++i0 )
{
Segment3<Real> segment;
segment.Center = mRectangle->Center +
( ( Real )( 2 * i0 - 1 ) ) * scaledDir[i1];
segment.Direction = mRectangle->Axis[1 - i1];
segment.Extent = mRectangle->Extent[1 - i1];
segment.ComputeEndPoints();
DistLine3Segment3<Real> queryLS( *mLine, segment );
Real sqrDistTmp = queryLS.GetSquared();
if ( sqrDistTmp < sqrDist )
{
mClosestPoint0 = queryLS.GetClosestPoint0();
mClosestPoint1 = queryLS.GetClosestPoint1();
sqrDist = sqrDistTmp;
mLineParameter = queryLS.GetLineParameter();
Real ratio = queryLS.GetSegmentParameter() / segment.Extent;
mRectCoord[0] = mRectangle->Extent[0] * ( ( 1 - i1 ) * ( 2 * i0 - 1 ) +
i1 * ratio );
mRectCoord[1] = mRectangle->Extent[1] * ( ( 1 - i0 ) * ( 2 * i1 - 1 ) +
i0 * ratio );
}
}
}
return sqrDist;
}
Real DistLine3Triangle3<Real>::GetSquared ()
{
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3<Real> edge0 = mTriangle->V[1] - mTriangle->V[0];
Vector3<Real> edge1 = mTriangle->V[2] - mTriangle->V[0];
Vector3<Real> normal = edge0.UnitCross(edge1);
Real NdD = normal.Dot(mLine->Direction);
if (Math<Real>::FAbs(NdD) > Math<Real>::ZERO_TOLERANCE)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3<Real> diff = mLine->Origin - mTriangle->V[0];
Vector3<Real> U, V;
Vector3<Real>::GenerateComplementBasis(U, V, mLine->Direction);
Real UdE0 = U.Dot(edge0);
Real UdE1 = U.Dot(edge1);
Real UdDiff = U.Dot(diff);
Real VdE0 = V.Dot(edge0);
Real VdE1 = V.Dot(edge1);
Real VdDiff = V.Dot(diff);
Real invDet = ((Real)1)/(UdE0*VdE1 - UdE1*VdE0);
// Barycentric coordinates for the point of intersection.
Real b1 = (VdE1*UdDiff - UdE1*VdDiff)*invDet;
Real b2 = (UdE0*VdDiff - VdE0*UdDiff)*invDet;
Real b0 = (Real)1 - b1 - b2;
if (b0 >= (Real)0 && b1 >= (Real)0 && b2 >= (Real)0)
{
// Line parameter for the point of intersection.
Real DdE0 = mLine->Direction.Dot(edge0);
Real DdE1 = mLine->Direction.Dot(edge1);
Real DdDiff = mLine->Direction.Dot(diff);
mLineParameter = b1*DdE0 + b2*DdE1 - DdDiff;
// Barycentric coordinates for the point of intersection.
mTriangleBary[0] = b0;
mTriangleBary[1] = b1;
mTriangleBary[2] = b2;
// The intersection point is inside or on the triangle.
mClosestPoint0 = mLine->Origin +
mLineParameter*mLine->Direction;
mClosestPoint1 = mTriangle->V[0] + b1*edge0 + b2*edge1;
return (Real)0;
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
Real sqrDist = Math<Real>::MAX_REAL;
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
Segment3<Real> segment;
segment.Center = ((Real)0.5)*(mTriangle->V[i0] +
mTriangle->V[i1]);
segment.Direction = mTriangle->V[i1] - mTriangle->V[i0];
segment.Extent = ((Real)0.5)*segment.Direction.Normalize();
segment.ComputeEndPoints();
DistLine3Segment3<Real> queryLS(*mLine, segment);
Real sqrDistTmp = queryLS.GetSquared();
if (sqrDistTmp < sqrDist)
{
mClosestPoint0 = queryLS.GetClosestPoint0();
mClosestPoint1 = queryLS.GetClosestPoint1();
sqrDist = sqrDistTmp;
mLineParameter = queryLS.GetLineParameter();
Real ratio = queryLS.GetSegmentParameter()/segment.Extent;
mTriangleBary[i0] = ((Real)0.5)*((Real)1 - ratio);
mTriangleBary[i1] = (Real)1 - mTriangleBary[i0];
mTriangleBary[3-i0-i1] = (Real)0;
}
}
return sqrDist;
}
