#include "Stdafx.h"
#include "EtCollisionFunc.h"
#include "EtCollisionPrimitive.h"
#include "EtComputeDist.h"

#ifdef _DEBUG
#define new new(_NORMAL_BLOCK,__FILE__,__LINE__)
#endif

bool TestLineToTriangle( EtVector3 &Origin, EtVector3 &Direction, EtVector3 &Point1, EtVector3 &Point2, EtVector3 &Point3, 
						float &fDist, float &fBary1, float &fBary2 )
{
	float fDeterminant;
	EtVector3 Edge1, Edge2;
	EtVector3 PVector, TVector, QVector;

	Edge1 = Point2 - Point1;
	Edge2 = Point3 - Point1;
	EtVec3Cross( &PVector, &Direction, &Edge2 );
	fDeterminant = EtVec3Dot( &Edge1, &PVector );

	fDist = FLT_MAX;
	if( fDeterminant > 0 )
	{
		TVector = Origin - Point1;
	}
	else
	{
		TVector = Point1 - Origin;
		fDeterminant = -fDeterminant;
	}
	if( fDeterminant < NEAR_ZERO )
	{
		return false;
	}

	fBary1 = EtVec3Dot( &TVector, &PVector );
	if( ( fBary1 < 0.0f ) || ( fBary1 > fDeterminant ) )
	{
		return false;
	}

	EtVec3Cross( &QVector, &TVector, &Edge1 );
	fBary2 = EtVec3Dot( &Direction, &QVector );
	if( ( fBary2 < 0.0f ) || ( fBary2 + fBary1 > fDeterminant ) )
	{
		return false;
	}

	fDist = EtVec3Dot( &Edge2, &QVector );
	fDist /= fDeterminant;
	fBary1 /= fDeterminant;
	fBary2 /= fDeterminant;

	return true;
}

bool TestLineToBox( EtVector3 &Origin, EtVector3 &Direction, SAABox &Box, float &fDist )
{
	bool bInside;

	fDist = -FLT_MAX;
	bInside = true;
	if( Box.Min.x - Origin.x > NEAR_ZERO )
	{
		if( Direction.x <= 0.0f )
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Min.x - Origin.x ) / Direction.x );
		}
	}
	else if( Origin.x - Box.Max.x > NEAR_ZERO )
	{
		if( Direction.x >= 0.0f )
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Max.x - Origin.x ) / Direction.x );
		}
	}

	if( Box.Min.y - Origin.y > NEAR_ZERO )
	{
		if( Direction.y <= 0.0f )
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Min.y - Origin.y ) / Direction.y );
		}
	}
	else if( Origin.y - Box.Max.y > NEAR_ZERO )
	{
		if( Direction.y >= 0.0f )
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Max.y - Origin.y ) / Direction.y );
		}
	}

	if( Box.Min.z - Origin.z > NEAR_ZERO )
	{
		if( Direction.z <= 0.0f )
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Min.z - Origin.z ) / Direction.z );
		}
	}
	else if( Origin.z - Box.Max.z > NEAR_ZERO )
	{
		if(Direction.z >= 0.0f)
		{
			return false;
		}
		else
		{
			bInside = false;
			fDist = max( fDist, ( Box.Max.z - Origin.z ) / Direction.z );
		}
	}

	if( bInside )
	{
		fDist = 0.0f;
		return true;
	}

	EtVector3 Hit;

	Hit = Origin + Direction * fDist;
	const float BOX_SIDE_THRESHOLD = 0.1f;
	if(	Hit.x > Box.Min.x - BOX_SIDE_THRESHOLD && Hit.x < Box.Max.x + BOX_SIDE_THRESHOLD &&
		Hit.y > Box.Min.y - BOX_SIDE_THRESHOLD && Hit.y < Box.Max.y + BOX_SIDE_THRESHOLD &&
		Hit.z > Box.Min.z - BOX_SIDE_THRESHOLD && Hit.z < Box.Max.z + BOX_SIDE_THRESHOLD)
		return true;

	return false;
}

bool TestSegmentToSphere( SSegment &Segment, SSphere &Sphere )
{
	float fDistSq, fSegParam;

	fDistSq = DistPointToSegment( Sphere.Center, Segment, fSegParam );

	return fDistSq <= Sphere.fRadius * Sphere.fRadius;
}

bool TestSegmentToOBB( SSegment &Segment, SOBB &Box )
{
	float fAWdU[ 3 ], fADdU[ 3 ], fAWxDdU[ 3 ], fRhs;
	EtVector3 HalfDir = 0.5f * Segment.vDirection;
	EtVector3 Center = Segment.vOrigin + HalfDir;

	EtVector3 Diff = Center - Box.Center;

	fAWdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 0 ] ) ) );
	fADdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 0 ] ) ) );
	fRhs = Box.Extent[ 0 ] + fAWdU[ 0 ];
	if ( fADdU[ 0 ] > fRhs )
	{
		return false;
	}

	fAWdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 1 ] ) ) );
	fADdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 1 ] ) ) );
	fRhs = Box.Extent[ 1 ] + fAWdU[ 1 ];
	if ( fADdU[ 1 ] > fRhs )
	{
		return false;
	}

	fAWdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &HalfDir, &Box.Axis[ 2 ] ) ) );
	fADdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &Diff, &Box.Axis[ 2 ] ) ) );
	fRhs = Box.Extent[ 2 ] + fAWdU[ 2 ];
	if ( fADdU[ 2 ] > fRhs )
	{
		return false;
	}

	EtVector3 WxD;
	EtVec3Cross( &WxD, &HalfDir, &Diff );

	fAWxDdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 0 ] ) ) );
	fRhs = Box.Extent[ 1 ]*fAWdU[ 2 ] + Box.Extent[ 2 ]*fAWdU[ 1 ];
	if ( fAWxDdU[ 0 ] > fRhs )
	{
		return false;
	}

	fAWxDdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 1 ] ) ) );
	fRhs = Box.Extent[ 0 ]*fAWdU[ 2 ] + Box.Extent[ 2 ]*fAWdU[ 0 ];
	if ( fAWxDdU[ 1 ] > fRhs )
	{
		return false;
	}

	fAWxDdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 2 ] ) ) );
	fRhs = Box.Extent[ 0 ]*fAWdU[ 1 ] + Box.Extent[ 1 ]*fAWdU[ 0 ];
	if ( fAWxDdU[ 2 ] > fRhs )
	{
		return false;
	}

	return true;
}

bool TestSegmentToCapsule( SSegment &Segment, SCapsule &Capsule )
{
	float fDistSq, fSegParam1, fSegParam2;

	fDistSq = DistSegmentToSegment( Segment, Capsule.Segment, fSegParam1, fSegParam2 );

	return fDistSq <= Capsule.fRadius * Capsule.fRadius;
}

bool TestLineToSphere( EtVector3 &Origin, EtVector3 &Direction, SSphere &Sphere )
{
	EtVector3 OriginToCenter;
	float fDot, fLength;

	OriginToCenter = Sphere.Center - Origin;
	fLength = EtVec3LengthSq( &OriginToCenter );
	EtVec3Normalize( &Direction, &Direction );
	fDot = EtVec3Dot( &OriginToCenter, &Direction );
	if( fDot < 0 )
	{
		return false;
	}
	if( Sphere.fRadius * Sphere.fRadius > fLength - fDot * fDot )
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool TestLineToPlane( EtVector3 &Origin, EtVector3 &Direction, EtVector4 &Plane, EtVector3 &IntersectPoint )
{
	float fDot;

	fDot = EtVec3Dot( ( EtVector3 * )&Plane, &Direction );
	if( FastAbs( fDot ) < NEAR_ZERO )
	{
		return false;
	}
	fDot = ( Plane.w - ( Plane.x * Origin.x + Plane.y * Origin.y + Plane.z * Origin.z ) ) / fDot;
	if( fDot < 0.0f )
	{
		return false;
	}
	IntersectPoint = Origin + Direction * fDot;

	return true;
}

bool TestEdgeToPlane( EtVector3 &Start, EtVector3 &End, EtVector4 &Plane, EtVector3 &IntersectPoint )
{
	EtVector3 Direction;
	float fDot;

	Direction = End - Start;
	fDot = EtVec3Dot( ( EtVector3 * )&Plane, &Direction );
	//	fDot = Plane.x * Direction.x + Plane.y * Direction.y + Plane.z * Direction.z;
	if( FastAbs( fDot ) < NEAR_ZERO )
	{
		return false;
	}
	fDot = ( Plane.w - ( Plane.x * Start.x + Plane.y * Start.y + Plane.z * Start.z ) ) / fDot;
	if( ( fDot < -NEAR_ZERO ) || ( fDot > 1.0f + NEAR_ZERO ) )
	{
		return false;
	}
	IntersectPoint = Start + Direction * fDot;

	return true;
}

bool IsBehindPoint( EtVector4 &Plane, EtVector3 &Point )
{	
	return Plane.x * Point.x + Plane.y * Point.y  + Plane.z * Point.z - Plane.w > NEAR_ZERO;
}

bool IsNearPoint( EtVector3 &Point1, EtVector3 &Point2, float fError )
{
	if( FastAbs( Point1.x - Point2.x ) > fError )
	{
		return false;
	}
	if( FastAbs( Point1.y - Point2.y ) > fError )
	{
		return false;
	}
	if( FastAbs( Point1.z - Point2.z ) > fError )
	{
		return false;
	}

	return true;
}

bool TestBoxToSphere( SAABox &Box, SSphere &Sphere )
{
	EtVector3 vBoxCenter, vBoxExtent, vDiff;
	float fAx, fAy, fAz, fDx, fDy, fDz, fSqrtRadius;

	vBoxExtent = ( Box.Max - Box.Min ) * 0.5f;
	vBoxCenter = ( Box.Max + Box.Min ) * 0.5f;
	vDiff = Sphere.Center - vBoxCenter;

	fAx = vDiff.x;
	fAy = vDiff.y;
	fAz = vDiff.z;
	fDx = fAx - vBoxExtent.x;
	fDy = fAy - vBoxExtent.y;
	fDz = fAz - vBoxExtent.z;

	if ( fAx <= vBoxExtent[ 0 ] )
	{
		if ( fAy <= vBoxExtent[ 1 ] )
		{
			if ( fAz <= vBoxExtent[ 2 ] )
			{
				return true;
			}
			else
			{
				return fDz <= Sphere.fRadius;
			}
		}
		else
		{
			if ( fAz <= vBoxExtent[ 2 ] )
			{
				return fDy <= Sphere.fRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDy * fDy + fDz * fDz <= fSqrtRadius;
			}
		}
	}
	else
	{
		if ( fAy <= vBoxExtent[ 1 ] )
		{
			if ( fAz <= vBoxExtent[ 2 ] )
			{
				return fDx <= Sphere.fRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDz*fDz <= fSqrtRadius;
			}
		}
		else
		{
			if ( fAz <= vBoxExtent[ 2 ] )
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDy*fDy <= fSqrtRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDy*fDy + fDz*fDz <= fSqrtRadius;
			}
		}
	}

/*	EtVector3 Center, Extent;

	Center = ( Box.Max + Box.Min ) * 0.5f;
	Extent = ( Box.Max - Box.Min ) * 0.5f;

	if( fabs( Center.x - Sphere.Center.x ) > ( Extent.x + Sphere.fRadius ) )
	{
		return false;
	}
	if( fabs( Center.y - Sphere.Center.y ) > ( Extent.y + Sphere.fRadius ) )
	{
		return false;
	}
	if( fabs( Center.z - Sphere.Center.z ) > ( Extent.z + Sphere.fRadius ) )
	{
		return false;
	}

	float fRootRadius;

	fRootRadius = Sphere.fRadius * 0.707106f;
	if( fabs( Center.x - Sphere.Center.x ) < ( Extent.x + fRootRadius ) )
	{
		return true;
	}
	if( fabs( Center.y - Sphere.Center.y ) > ( Extent.y + fRootRadius ) )
	{
		return true;
	}
	if( fabs( Center.z - Sphere.Center.z ) > ( Extent.z + fRootRadius ) )
	{
		return true;
	}

	return false;*/
}

bool TestSphereToSphere( SSphere &Sphere1, SSphere &Sphere2 )
{
	float fLengthSq;

	fLengthSq = EtVec3LengthSq( &( Sphere1.Center - Sphere2.Center ) );
	if( fLengthSq > ( Sphere1.fRadius + Sphere2.fRadius ) * ( Sphere1.fRadius + Sphere2.fRadius ) )
	{
		return false;
	}
	return true;
}

bool TestSphereToCapsule( SSphere &Sphere, SCapsule &Capsule, float &fSegParam )
{
	float fLengthSq;

	fLengthSq = DistPointToSegment( Sphere.Center, Capsule.Segment, fSegParam );
	
	return fLengthSq <= ( Capsule.fRadius + Sphere.fRadius ) * ( Capsule.fRadius + Sphere.fRadius );
}

bool TestBoxToBox( SAABox &Box1, SAABox &Box2 )
{
	EtVector3 Center1, Center2;
	EtVector3 Extent1, Extent2;

	Center1 = ( Box1.Max + Box1.Min ) * 0.5f;
	Center2 = ( Box2.Max + Box2.Min ) * 0.5f;
	Extent1 = ( Box1.Max - Box1.Min ) * 0.5f;
	Extent2 = ( Box2.Max - Box2.Min ) * 0.5f;

	if( FastAbs( Center1.x - Center2.x ) > ( Extent1.x + Extent2.x ) )
	{
		return false;
	}
	if( FastAbs( Center1.y - Center2.y ) > ( Extent1.y + Extent2.y ) )
	{
		return false;
	}
	if( FastAbs( Center1.z - Center2.z ) > ( Extent1.z + Extent2.z ) )
	{
		return false;
	}

	return true;
}

bool TestCircleToCircle( SCircle &Circle1, SCircle &Circle2 )
{
	float fRadiusSum;

	fRadiusSum = Circle1.fRadius + Circle2.fRadius;
	if( EtVec2LengthSq( &( Circle1.Center - Circle2.Center ) ) < fRadiusSum * fRadiusSum )
	{
		return true;
	}
	return false;
}

bool TestCircleToBox2D( SCircle &Circle, SAABox2D &Box )
{
	int i;
	float fRadiusSq;
	DNVector(EtVector2) vecVertex;
	Box.GetVertices( vecVertex );
	fRadiusSq = Circle.fRadius * Circle.fRadius;
	for( i = 0; i < ( int )vecVertex.size(); i++ )
	{
		if( EtVec2LengthSq( &( vecVertex[ i ] - Circle.Center ) ) < fRadiusSq )
		{
			return true;
		}
	}
	if( Box.IsInside( Circle ) )
	{
		return true;
	}

	return false;
}

bool TestLineToOBB( EtVector3 &Origin, EtVector3 &Direction, SOBB &Box )
{
	float fWdU[ 3 ], fAWdU[ 3 ], fDdU[ 3 ], fADdU[ 3 ], fAWxDdU[ 3 ], fRhs;

	EtVector3 Diff = Origin - Box.Center;

	fWdU[ 0 ] =  EtVec3Dot( &Direction, &Box.Axis[ 0 ]);
	fAWdU[ 0 ] = ( float )FastAbs( ( float )(fWdU[ 0 ]) );
	fDdU[ 0 ] =  EtVec3Dot( &Diff, &Box.Axis[ 0 ] );
	fADdU[ 0 ] = ( float )FastAbs( ( float )(fDdU[ 0 ]) );
	if ( fADdU[ 0 ] > Box.Extent[ 0 ] && fDdU[ 0 ] * fWdU[ 0 ] >= 0.0f )
	{
		return false;
	}

	fWdU[ 1 ] =  EtVec3Dot( &Direction, &Box.Axis[ 1 ]);
	fAWdU[ 1 ] = ( float )FastAbs( ( float )(fWdU[ 1 ]) );
	fDdU[ 1 ] =  EtVec3Dot( &Diff, &Box.Axis[ 1 ]);
	fADdU[ 1 ] = ( float )FastAbs( ( float )(fDdU[ 1 ]) );
	if ( fADdU[ 1 ] > Box.Extent[ 1 ] && fDdU[ 1 ] * fWdU[ 1 ] >= 0.0f )
	{
		return false;
	}

	fWdU[ 2 ] =  EtVec3Dot( &Direction, &Box.Axis[ 2 ]);
	fAWdU[ 2 ] = ( float )FastAbs( ( float )(fWdU[ 2 ]) );
	fDdU[ 2 ] =  EtVec3Dot( &Diff, &Box.Axis[ 2 ]);
	fADdU[ 2 ] = ( float )FastAbs( ( float )(fDdU[ 2 ]) );
	if ( fADdU[ 2 ] > Box.Extent[ 2 ] && fDdU[ 2 ] * fWdU[ 2 ] >= 0.0f )
	{
		return false;
	}

	EtVector3 WxD;
	EtVec3Cross( &WxD, &Direction, &Diff );

	fAWxDdU[ 0 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 0 ] ) ) );
	fRhs = Box.Extent[ 1 ] * fAWdU[ 2 ] + Box.Extent[ 2 ] * fAWdU[ 1 ];
	if ( fAWxDdU[ 0 ] > fRhs )
	{
		return false;
	}

	fAWxDdU[ 1 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 1 ] ) ) );
	fRhs = Box.Extent[ 0 ] * fAWdU[ 2 ] + Box.Extent[ 2 ] * fAWdU[ 0 ];
	if ( fAWxDdU[ 1 ] > fRhs )
	{
		return false;
	}

	fAWxDdU[ 2 ] = ( float )FastAbs(( float )( EtVec3Dot( &WxD, &Box.Axis[ 2 ] ) ) );
	fRhs = Box.Extent[ 0 ] * fAWdU[ 1 ] + Box.Extent[ 1 ] * fAWdU[ 0 ];
	if ( fAWxDdU[ 2 ] > fRhs )
	{
		return false;
	}

	return true;
}

bool TestOBBToOBB( SOBB &Box1, SOBB &Box2 )
{
	EtVector3 Min, Max;
	float fExtentDiagonal[3];

	Box1.GetDiagonal( Min, Max, Box2.Axis[ 0 ] );
	fExtentDiagonal[ 0 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) *0.5f ), Box2.Axis ) );

	Box1.GetDiagonal( Min, Max, Box2.Axis[ 1 ] );
	fExtentDiagonal[ 1 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) * 0.5f ), Box2.Axis + 1 ) );

	Box1.GetDiagonal( Min, Max, Box2.Axis[ 2 ] );
	fExtentDiagonal[ 2 ] = FastAbs( EtVec3Dot( &( ( Max - Min ) * 0.5f ), Box2.Axis + 2 ) );

	EtVector3 CenterDist;

	CenterDist = Box1.Center - Box2.Center;
	if( Box2.Extent[ 0 ] + fExtentDiagonal[ 0 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis ) ) )
	{
		return false;
	}
	if( Box2.Extent[ 1 ] + fExtentDiagonal[ 1 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis + 1 ) ) )	
	{
		return false;
	}
	if( Box2.Extent[ 2 ] + fExtentDiagonal[ 2 ] < FastAbs( EtVec3Dot( &CenterDist, Box2.Axis + 2 ) ) )
	{
		return false;
	}

	return true;
}

bool TestOBBToSphere( SOBB &Box, SSphere &Sphere )
{
	EtVector3 Diff = Sphere.Center - Box.Center;

	float fAx = FastAbs( EtVec3Dot( &Diff, Box.Axis ) );
	float fAy = FastAbs( EtVec3Dot( &Diff, Box.Axis + 1 ) );
	float fAz = FastAbs( EtVec3Dot( &Diff, Box.Axis + 2 ) );
	float fDx = fAx - Box.Extent[ 0 ];
	float fDy = fAy - Box.Extent[ 1 ];
	float fDz = fAz - Box.Extent[ 2 ];
	float fSqrtRadius;

	if ( fAx <= Box.Extent[ 0 ] )
	{
		if ( fAy <= Box.Extent[ 1 ] )
		{
			if ( fAz <= Box.Extent[ 2 ] )
			{
				return true;
			}
			else
			{
				return fDz <= Sphere.fRadius;
			}
		}
		else
		{
			if ( fAz <= Box.Extent[ 2 ] )
			{
				return fDy <= Sphere.fRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDy * fDy + fDz * fDz <= fSqrtRadius;
			}
		}
	}
	else
	{
		if ( fAy <= Box.Extent[ 1 ] )
		{
			if ( fAz <= Box.Extent[ 2 ] )
			{
				return fDx <= Sphere.fRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDz*fDz <= fSqrtRadius;
			}
		}
		else
		{
			if ( fAz <= Box.Extent[ 2 ] )
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDy*fDy <= fSqrtRadius;
			}
			else
			{
				fSqrtRadius = Sphere.fRadius * Sphere.fRadius;
				return fDx*fDx + fDy*fDy + fDz*fDz <= fSqrtRadius;
			}
		}
	}
}

/*bool TestOBBToCapsule( SOBB &Box, SCapsule &Capsule, float &fSegParam )
{
	float fLengthSq;
	float fBoxParam0, fBoxParam1, fBoxParam2;

	fLengthSq = DistSegToOBB( Capsule.Segment, Box, fSegParam, fBoxParam0, fBoxParam1, fBoxParam2 );

	return fLengthSq <= Capsule.fRadius * Capsule.fRadius;
}*/

bool TestCapsuleToCapsule( SCapsule &Capsule1, SCapsule &Capsule2, float &fSegParam1, float &fSegParam2 )
{
	float fLengthSq;

	fLengthSq = DistSegmentToSegment( Capsule1.Segment, Capsule2.Segment, fSegParam1, fSegParam2 );

	return fLengthSq <= ( Capsule1.fRadius + Capsule2.fRadius ) * ( Capsule1.fRadius + Capsule2.fRadius );
}

bool CalcFrustumPlane( EtVector4 &Out, float fX, float fY, float fZ, float fW )
{
	float fLength, fInvLength;

	fLength = sqrtf( fX * fX + fY * fY + fZ * fZ );
	if( fLength < NEAR_ZERO )
	{
		return false;
	}

	fInvLength = 1.0f / fLength;
	Out = EtVector4( -fX * fInvLength, -fY * fInvLength, -fZ * fInvLength, fW * fInvLength );

	return true;
}