geometry3d.cpp

手机 ＧＡＭＥ ｃ＋＋ 版

////////////////////////////////////////////////////////////////////////
//
// Geometry3D.cpp
//
// Copyright (c) 2003 Nokia Mobile Phones Ltd.  All rights reserved.
//
////////////////////////////////////////////////////////////////////////

#include "Geometry3D.h"
#include "MathLookup.h"

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

TInt Geometry3D::Reduce(TInt &aSubject, TInt aLimit)
	{
	TInt compare = aLimit;
	TInt shift = 0;

	while ( compare < aSubject )
		{
		compare <<= 1;
		shift += 1;
		}

	if ( shift > 0 )
		{
		TInt bias = 1 << ( shift - 1 );
		aSubject = ( aSubject + bias ) >> shift;
		}

	return shift;
	}

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

TVector3::TVector3()
	{
	iX = 0;
	iY = 0;
	iZ = 0;
	}

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

TVector3::TVector3(TInt aX, TInt aY, TInt aZ)
	{
	iX = aX;
	iY = aY;
	iZ = aZ;
	}

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

void TVector3::MakeSum(const TVector3 &aVectorA, const TVector3 &aVectorB)
	{
	iX = aVectorA.iX + aVectorB.iX;
	iY = aVectorA.iY + aVectorB.iY;
	iZ = aVectorA.iZ + aVectorB.iZ;
	}

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

void TVector3::MakeDifference(const TVector3 &aVectorA, const TVector3 &aVectorB)
	{
	iX = aVectorA.iX - aVectorB.iX;
	iY = aVectorA.iY - aVectorB.iY;
	iZ = aVectorA.iZ - aVectorB.iZ;
	}

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

void TVector3::MakeInverse(const TVector3 &aVector)
	{
	iX = -aVector.iX;
	iY = -aVector.iY;
	iZ = -aVector.iZ;
	}

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

void TVector3::MakeCrossProduct(const TVector3 &aVectorA, const TVector3 &aVectorB)
	{
	iX = ( aVectorB.iY * aVectorA.iZ ) - ( aVectorA.iY * aVectorB.iZ );
	iY = ( aVectorB.iZ * aVectorA.iX ) - ( aVectorA.iZ * aVectorB.iX );
	iZ = ( aVectorB.iX * aVectorA.iY ) - ( aVectorA.iX * aVectorB.iY );
	}

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

void TVector3::ScaleFrom(const TVector3 &aVector, TInt aScale)
	{
	iX = aVector.iX * aScale;
	iY = aVector.iY * aScale;
	iZ = aVector.iZ * aScale;
	}

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

void TVector3::ScaleFrom(const TVector3 &aVector, TInt aScale, TInt aDownshift)
	{
	TInt bias = 1 << ( aDownshift - 1 );
	iX = ( ( aVector.iX * aScale ) + bias ) >> aDownshift;
	iY = ( ( aVector.iY * aScale ) + bias ) >> aDownshift;
	iZ = ( ( aVector.iZ * aScale ) + bias ) >> aDownshift;
	}

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

void TVector3::NormaliseFrom(const TVector3 &aVector, TInt aUnity)
	{
	TReal x = aVector.iX;
	TReal y = aVector.iY;
	TReal z = aVector.iZ;

	TReal magnitudeSquared = ( x * x ) + ( y * y ) + ( z * z );
	TReal magnitude;
	Math::Sqrt(magnitude, magnitudeSquared);

	TReal ratio = aUnity / magnitude;

	x *= ratio;
	y *= ratio;
	z *= ratio;

	TInt32 xAsInt;
	TInt32 yAsInt;
	TInt32 zAsInt;

	Math::Int(xAsInt, x);
	Math::Int(yAsInt, y);
	Math::Int(zAsInt, z);

	iX = xAsInt;
	iY = yAsInt;
	iZ = zAsInt;
	}

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

void TVector3::ReduceFrom(const TVector3 &aVector, TInt aUnity)
	{
	*this = aVector;

	TInt absX = iX > 0 ? iX : -iX;
	TInt absY = iY > 0 ? iY : -iY;
	TInt absZ = iZ > 0 ? iZ : -iZ;

	// Get "Manhattan magnitude" of vector:
	TInt sumAbs = absX + absY + absZ;

	TInt orComp = iX | iY | iZ;

	// Is vector too large?
	// Do the components have trailing zeroes we can discard?
	while ( ( sumAbs > aUnity ) || ( ( orComp & 0x01 ) == 0 ) )
		{
		iX >>= 1;
		iY >>= 1;
		iZ >>= 1;

		sumAbs >>= 1;
		orComp >>= 1;
		}
	}

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

TInt TVector3::DotProduct(const TVector3 &aVectorA, const TVector3 &aVectorB)
	{
	return ( aVectorA.iX * aVectorB.iX ) + ( aVectorA.iY * aVectorB.iY ) 
												+ ( aVectorA.iZ * aVectorB.iZ ); 
	}

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

TPlane::TPlane(const TVector3 &aVectorA, const TVector3 &aVectorB, const TVector3 &aVectorC)
	{
	// Generate normal vector by finding two edge vectors and taking their cross product:
	TVector3 edgeVectorA;
	TVector3 edgeVectorB;

	edgeVectorA.MakeDifference(aVectorB, aVectorA);
	edgeVectorB.MakeDifference(aVectorB, aVectorC);

	TVector3 edgeCrossProduct;

	edgeCrossProduct.MakeCrossProduct(edgeVectorA, edgeVectorB);
	iNormalVector.ReduceFrom(edgeCrossProduct, planeNormalUnity);

	// Take position of face (any point will do) and dot with normal:
	iAltitude = TVector3::DotProduct(aVectorA, iNormalVector);
	}

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

TBool TPlane::PointIsOutside(const TVector3 &aPoint)
	{
	TInt pointValue = TVector3::DotProduct(aPoint, iNormalVector);
	return ( pointValue > iAltitude );
	}

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

void TPlane::IntersectLineSegment
	( 
	TVector3 &aStart, 
	TVector3 &aEnd, 
	TVector3 &aOutput, 
	TMathLookup &aMath 
	)
	{
	TInt startAltitude	= TVector3::DotProduct(aStart, iNormalVector);
	TInt endAltitude	= TVector3::DotProduct(aEnd, iNormalVector);

	TVector3 * insidePtr;
	TVector3 * outsidePtr;

	TInt inAltitude;
	TInt outAltitude;

	if ( endAltitude > startAltitude )
		{
		insidePtr = &aStart;
		outsidePtr = &aEnd;

		inAltitude = startAltitude;
		outAltitude = endAltitude;
		}
	else
		{
		insidePtr = &aEnd;
		outsidePtr = &aStart;

		inAltitude = endAltitude;
		outAltitude = startAltitude;
		}

	TVector3 segmentVec;
	segmentVec.MakeDifference( *outsidePtr, *insidePtr );

	// Total change in altitude of segment:
	TInt segmentDeltaAltitude = outAltitude - inAltitude;

	// Change in altitude for portion of segment *outside* plane:
	TInt excessDeltaAltitude = outAltitude - iAltitude;

	//
	// We now need to scale segmentVec by the ratio of excess length to segment length,
	// but the constraints of fixed point (need for precision vs. need to avoid overflows)
	// make this a drawn-out process:
	//

	TInt recip;
	TInt shift;

	aMath.GetReciprocal(segmentDeltaAltitude, recip, shift);

	// Before deriving the ratio, we must make sure that excessDeltaAltitude is not
	// so large that the calculation might overflow:

	TInt excessDeltaShift = Geometry3D::Reduce(excessDeltaAltitude, maxExcessAltitude);
	shift -= excessDeltaShift;

	TInt excessToSegmentRatio = excessDeltaAltitude * recip;

	// Before using this ratio, we must ensure that it is small enough to avoid
	// overflows in TVector3::ScaleFrom(): it is probably much too big as it stands.
	TInt excessRatioShift = Geometry3D::Reduce(excessToSegmentRatio, maxExcessRatio);
	shift -= excessRatioShift;

	// Do we have a degenerate case?
	if ( shift >= 31 )
		{
		aOutput = *outsidePtr;
		}
	else
		{
		// Now we can perform the scale itself:
		TVector3 excessVec;
		excessVec.ScaleFrom(segmentVec, excessToSegmentRatio, shift);

		// Now we can backtrack from the outside end of the segment to the 
		// intersection point:
		aOutput.MakeDifference(*outsidePtr, excessVec);
		}
	}

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

TMatrix3x3::TMatrix3x3()
	{
	MakeIdentity();
	}

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

void TMatrix3x3::MakeIdentity() 
	{
	for ( TInt j = 0 ; j < 3 ; j++ )
		{
		for ( TInt i = 0 ; i < 3 ; i++ )
			{
			if ( i == j )
				{
				iElement[ i ][ j ] = matrixUnity;
				}
			else
				{
				iElement[ i ][ j ] = 0;
				}
			}
		}
	}

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

void TMatrix3x3::MakeRx(TInt aAngle, TMathLookup &aMath) 
	{
	TInt cos;
	TInt sin;
	
	aMath.GetTrigRatios(aAngle, cos, sin);

	iElement[ 0 ][ 0 ] = matrixUnity;
	iElement[ 0 ][ 1 ] = 0;
	iElement[ 0 ][ 2 ] = 0;

	iElement[ 1 ][ 0 ] = 0;
	iElement[ 1 ][ 1 ] = cos;
	iElement[ 1 ][ 2 ] = -sin;

	iElement[ 2 ][ 0 ] = 0;
	iElement[ 2 ][ 1 ] = sin;
	iElement[ 2 ][ 2 ] = cos;
	}

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

void TMatrix3x3::MakeRy(TInt aAngle, TMathLookup &aMath) 
	{
	TInt cos;
	TInt sin;
	
	aMath.GetTrigRatios(aAngle, cos, sin);

	iElement[ 0 ][ 0 ] = cos;
	iElement[ 0 ][ 1 ] = 0;
	iElement[ 0 ][ 2 ] = sin;

	iElement[ 1 ][ 0 ] = 0;
	iElement[ 1 ][ 1 ] = matrixUnity;
	iElement[ 1 ][ 2 ] = 0;

	iElement[ 2 ][ 0 ] = -sin;
	iElement[ 2 ][ 1 ] = 0;
	iElement[ 2 ][ 2 ] = cos;
	}

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

void TMatrix3x3::MakeRz(TInt aAngle, TMathLookup &aMath) 
	{
	TInt cos;
	TInt sin;
	
	aMath.GetTrigRatios(aAngle, cos, sin);

	iElement[ 0 ][ 0 ] = cos;
	iElement[ 0 ][ 1 ] = -sin;
	iElement[ 0 ][ 2 ] = 0;

	iElement[ 1 ][ 0 ] = sin;
	iElement[ 1 ][ 1 ] = cos;
	iElement[ 1 ][ 2 ] = 0;

	iElement[ 2 ][ 0 ] = 0;
	iElement[ 2 ][ 1 ] = 0;
	iElement[ 2 ][ 2 ] = matrixUnity;
	}

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

void TMatrix3x3::MakeCompound(const TMatrix3x3 &aMatrixA, const TMatrix3x3 &aMatrixB) 
	{
	TMatrix3x3 result;

	for ( TInt j = 0 ; j < 3 ; j++ )
		{
		for ( TInt i = 0 ; i < 3 ; i++ )
			{
			result.iElement[ i ][ j ] =	(
										  aMatrixA.iElement[ 0 ][ j ] * aMatrixB.iElement[ i ][ 0 ]
										+ aMatrixA.iElement[ 1 ][ j ] * aMatrixB.iElement[ i ][ 1 ]
										+ aMatrixA.iElement[ 2 ][ j ] * aMatrixB.iElement[ i ][ 2 ]
										) >> matrixUnityLog;
			}
		}

	*this = result;
	}

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

void TMatrix3x3::MakeInverse(const TMatrix3x3 &aSourceMatrix) 
	{
	// Takes inverse of a given matrix by exchanging rows and columns.
	// Note that this method is valid for pure rotation matrices,
	// but not for scaling or scale-and-rotate matrices.
	TMatrix3x3 result;

	for ( TInt j = 0 ; j < 3 ; j++ )
		{
		for ( TInt i = 0 ; i < 3 ; i++ )
			{
			result.iElement[ i ][ j ] = aSourceMatrix.iElement[ j ][ i ];
			}
		}

	*this = result;
	}

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

void TMatrix3x3::Transform(const TVector3 &aSourceVector, TVector3 &aDestVector) const
	{
	// We want to allow source and destination to be identical, so we
	// generate a temporary result and copy this to the destination:
	TVector3 result;

	result.iX = 	(
					  aSourceVector.iX * iElement[ 0 ][ 0 ]
					+ aSourceVector.iY * iElement[ 0 ][ 1 ]
					+ aSourceVector.iZ * iElement[ 0 ][ 2 ]
					) >> matrixUnityLog;

	result.iY = 	(
					  aSourceVector.iX * iElement[ 1 ][ 0 ]
					+ aSourceVector.iY * iElement[ 1 ][ 1 ]
					+ aSourceVector.iZ * iElement[ 1 ][ 2 ]
					) >> matrixUnityLog;

	result.iZ = 	(
					  aSourceVector.iX * iElement[ 2 ][ 0 ]
					+ aSourceVector.iY * iElement[ 2 ][ 1 ]
					+ aSourceVector.iZ * iElement[ 2 ][ 2 ]
					) >> matrixUnityLog;

	aDestVector = result;
	}

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

void TMatrix3x3::InverseTransform(const TVector3 &aSourceVector, TVector3 &aDestVector) const
	{
	// The same as Transform(), but with rows and columns reversed.
	// Note that, as with MakeInverse(), this assumes that the matrix
	// does not contain a scaling factor.

	TVector3 result;

	result.iX = 	(
					  aSourceVector.iX * iElement[ 0 ][ 0 ]
					+ aSourceVector.iY * iElement[ 1 ][ 0 ]
					+ aSourceVector.iZ * iElement[ 2 ][ 0 ]
					) >> matrixUnityLog;			       
														   
	result.iY = 	(									       
					  aSourceVector.iX * iElement[ 0 ][ 1 ]
					+ aSourceVector.iY * iElement[ 1 ][ 1 ]
					+ aSourceVector.iZ * iElement[ 2 ][ 1 ]
					) >> matrixUnityLog;			       
														   
	result.iZ = 	(									       
					  aSourceVector.iX * iElement[ 0 ][ 2 ]
					+ aSourceVector.iY * iElement[ 1 ][ 2 ]
					+ aSourceVector.iZ * iElement[ 2 ][ 2 ]
					) >> matrixUnityLog;

	aDestVector = result;
	}

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

TAffineTransform::TAffineTransform(const TVector3 &aVector)
	{
	SetVector(aVector);
	}

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

TAffineTransform::TAffineTransform(const TMatrix3x3 &aMatrix)
	{
	SetMatrix(aMatrix);
	}

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

void TAffineTransform::SetVector(const TVector3 &aVector)
	{
	iVector = aVector;
	}

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

void TAffineTransform::SetMatrix(const TMatrix3x3 &aMatrix)
	{
	iMatrix = aMatrix;
	}

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

void TAffineTransform::MakeCompound
	(
	const TAffineTransform &aAffineA, 
	const TAffineTransform &aAffineB
	)
	{
	TAffineTransform result;

	result.iMatrix.MakeCompound(aAffineA.iMatrix, aAffineB.iMatrix);
	aAffineB.Transform(aAffineA.iVector, result.iVector);

	*this = result;
	}

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

void TAffineTransform::Transform(const TVector3 &aSourceVector, TVector3 &aDestVector) const
	{
	TVector3 scratchVector;

	iMatrix.Transform(aSourceVector, scratchVector);
	aDestVector.MakeSum(scratchVector, iVector);
	}

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

void TAffineTransform::GetInverseVector(TVector3 &aDestVector) const
	{
	// Apply inverse transform to origin (used to map camera into model space):
	TVector3 recipVec(-iVector.iX, -iVector.iY, -iVector.iZ);
	iMatrix.InverseTransform(recipVec, aDestVector);
	}

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