/// <summary>
/// Copy constructor
/// </summary>
public InvariantMatrix44( InvariantMatrix44 src )
{
Arguments.CheckNotNull( src, "src" );
for ( int index = 0; index < 16; ++index )
{
m_Elements[ index ] = src.m_Elements[ index ];
}
}
/// <summary>
/// Applies camera transforms to the current renderer
/// </summary>
public override void Begin( )
{
IRenderer renderer = Graphics.Renderer;
int width = renderer.ViewportWidth;
int height = renderer.ViewportHeight;
float aspectRatio = ( height == 0 ) ? 1.0f : width / ( float )height;
Graphics.Renderer.PushTransform( TransformType.ViewToScreen );
Graphics.Renderer.SetPerspectiveProjectionTransform( m_PerspectiveFov, aspectRatio, m_PerspectiveZNear, m_PerspectiveZFar );
ProjectionMatrix = Graphics.Renderer.GetTransform( TransformType.ViewToScreen );
// TODO: AP: Projection matrix should be updated by projection property sets
m_InvProjView.StoreMultiply( m_ProjectionMatrix, ViewMatrix );
m_InvProjView.Invert( );
base.Begin( );
}
/// <summary>
/// Sets up a matrix as a look-at matrix
/// </summary>
protected static void MakeLookAtMatrix( InvariantMatrix44 matrix, Point3 origin, Point3 lookAt, Vector3 up )
{
Vector3 zAxis = ( origin - lookAt ).MakeNormal( );
Vector3 yAxis = up;
Vector3 xAxis = Vector3.Cross( yAxis, zAxis );
yAxis = Vector3.Cross( zAxis, xAxis );
InvariantMatrix44 frame = new InvariantMatrix44(xAxis, yAxis, zAxis, Point3.Origin);
InvariantMatrix44 translation = MakeTranslationMatrix( -origin );
Multiply( matrix, frame, translation );
}
/// <summary>
/// Stores a quaternion in the specified matrix
/// </summary>
protected static void MakeQuaternionMatrix( InvariantMatrix44 matrix, Quaternion quaternion )
{
float fTx = 2.0f * quaternion.X;
float fTy = 2.0f * quaternion.Y;
float fTz = 2.0f * quaternion.Z;
float fTwx = fTx * quaternion.W;
float fTwy = fTy * quaternion.W;
float fTwz = fTz * quaternion.W;
float fTxx = fTx * quaternion.X;
float fTxy = fTy * quaternion.X;
float fTxz = fTz * quaternion.X;
float fTyy = fTy * quaternion.Y;
float fTyz = fTz * quaternion.Y;
float fTzz = fTz * quaternion.Z;
float[] elements = matrix.m_Elements;
elements[ 0 ] = 1.0f - ( fTyy + fTzz );
elements[ 1 ] = fTxy - fTwz;
elements[ 2 ] = fTxz + fTwy;
elements[ 4 ] = fTxy + fTwz;
elements[ 5 ] = 1.0f - ( fTxx + fTzz );
elements[ 6 ] = fTyz - fTwx;
elements[ 8 ] = fTxz - fTwy;
elements[ 9 ] = fTyz + fTwx;
elements[ 10 ] = 1.0f - ( fTxx + fTyy );
}
/// <summary>
/// Applies the specified transform, adds it to the specified transform stack
/// </summary>
public override void SetTransform( TransformType type, InvariantMatrix44 matrix )
{
switch ( type )
{
case TransformType.LocalToWorld :
{
CurrentLocalToWorld.Copy( matrix );
UpdateModelView( );
break;
}
case TransformType.WorldToView :
{
CurrentWorldToView.Copy( matrix );
UpdateModelView( );
break;
}
default :
{
SetSupportedMatrixMode( type );
Gl.glLoadMatrixf( GetGlMatrix( matrix ) );
break;
}
}
}
/// <summary>
/// Stores the inverse of src in result
/// </summary>
/// <remarks>
/// The generic inverse of a matrix A is found by dividing the adjoint of A by the determinant of A. The adjoint B of a matrix A is
/// defined by B=bij, where bij is the determinant of A with row i and column j removed (the co-factors of A).
/// I was too lazy to fully expand the calculations this implies for a 4x4 matrix, so I grabbed and adapted the following code from
/// http://www.geometrictools.com
/// </remarks>
protected static void MakeInverse( InvariantMatrix44 result, InvariantMatrix44 src )
{
// The inverse of an nxn matrix A is the adjoint of A divided through by the determinant of A
// Because the adjoint and determinant use the same sub-matrix determinants, we can store these values and use them in both calculations:
float[] elements = result.Elements;
float[] srcElements = src.Elements;
float a0 = srcElements[ 0 ] * srcElements[ 5 ] - srcElements[ 1 ] * srcElements[ 4 ];
float a1 = srcElements[ 0 ] * srcElements[ 6 ] - srcElements[ 2 ] * srcElements[ 4 ];
float a2 = srcElements[ 0 ] * srcElements[ 7 ] - srcElements[ 3 ] * srcElements[ 4 ];
float a3 = srcElements[ 1 ] * srcElements[ 6 ] - srcElements[ 2 ] * srcElements[ 5 ];
float a4 = srcElements[ 1 ] * srcElements[ 7 ] - srcElements[ 3 ] * srcElements[ 5 ];
float a5 = srcElements[ 2 ] * srcElements[ 7 ] - srcElements[ 3 ] * srcElements[ 6 ];
float b0 = srcElements[ 8 ] * srcElements[ 13 ] - srcElements[ 9 ] * srcElements[ 12 ];
float b1 = srcElements[ 8 ] * srcElements[ 14 ] - srcElements[ 10 ] * srcElements[ 12 ];
float b2 = srcElements[ 8 ] * srcElements[ 15 ] - srcElements[ 11 ] * srcElements[ 12 ];
float b3 = srcElements[ 9 ] * srcElements[ 14 ] - srcElements[ 10 ] * srcElements[ 13 ];
float b4 = srcElements[ 9 ] * srcElements[ 15 ] - srcElements[ 11 ] * srcElements[ 13 ];
float b5 = srcElements[ 10 ] * srcElements[ 15 ] - srcElements[ 11 ] * srcElements[ 14 ];
float det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
if ( ( det > -0.000001f ) && ( det < 0.000001f ) )
{
// Maybe store zero matrix instead?
throw new ApplicationException( "Tried to take the inverse of a matrix with determinant of zero" );
}
// Store the reciprocal of the determinant
float rcpDet = 1.0f / det;
// Store the adjoint of mat in this matrix
elements[ 0 ] = ( +srcElements[ 5 ] * b5 - srcElements[ 6 ] * b4 + srcElements[ 7 ] * b3 ) * rcpDet;
elements[ 4 ] = ( -srcElements[ 4 ] * b5 + srcElements[ 6 ] * b2 - srcElements[ 7 ] * b1 ) * rcpDet;
elements[ 8 ] = ( +srcElements[ 4 ] * b4 - srcElements[ 5 ] * b2 + srcElements[ 7 ] * b0 ) * rcpDet;
elements[ 12 ] = ( -srcElements[ 4 ] * b3 + srcElements[ 5 ] * b1 - srcElements[ 6 ] * b0 ) * rcpDet;
elements[ 1 ] = ( -srcElements[ 1 ] * b5 + srcElements[ 2 ] * b4 - srcElements[ 3 ] * b3 ) * rcpDet;
elements[ 5 ] = ( +srcElements[ 0 ] * b5 - srcElements[ 2 ] * b2 + srcElements[ 3 ] * b1 ) * rcpDet;
elements[ 9 ] = ( -srcElements[ 0 ] * b4 + srcElements[ 1 ] * b2 - srcElements[ 3 ] * b0 ) * rcpDet;
elements[ 13 ] = ( +srcElements[ 0 ] * b3 - srcElements[ 1 ] * b1 + srcElements[ 2 ] * b0 ) * rcpDet;
elements[ 2 ] = ( +srcElements[ 13 ] * a5 - srcElements[ 14 ] * a4 + srcElements[ 15 ] * a3 ) * rcpDet;
elements[ 6 ] = ( -srcElements[ 12 ] * a5 + srcElements[ 14 ] * a2 - srcElements[ 15 ] * a1 ) * rcpDet;
elements[ 10 ] = ( +srcElements[ 12 ] * a4 - srcElements[ 13 ] * a2 + srcElements[ 15 ] * a0 ) * rcpDet;
elements[ 14 ] = ( -srcElements[ 12 ] * a3 + srcElements[ 13 ] * a1 - srcElements[ 14 ] * a0 ) * rcpDet;
elements[ 3 ] = ( -srcElements[ 9 ] * a5 + srcElements[ 10 ] * a4 - srcElements[ 11 ] * a3 ) * rcpDet;
elements[ 7 ] = ( +srcElements[ 8 ] * a5 - srcElements[ 10 ] * a2 + srcElements[ 11 ] * a1 ) * rcpDet;
elements[ 11 ] = ( -srcElements[ 8 ] * a4 + srcElements[ 9 ] * a2 - srcElements[ 11 ] * a0 ) * rcpDet;
elements[ 15 ] = ( +srcElements[ 8 ] * a3 - srcElements[ 9 ] * a1 + srcElements[ 10 ] * a0 ) * rcpDet;
}
/// <summary>
/// Stores the multiple of lhs by rhs in result
/// </summary>
protected static void Multiply( InvariantMatrix44 result, InvariantMatrix44 lhs, InvariantMatrix44 rhs )
{
float[] elements = result.Elements;
for ( int row = 0; row < 4; ++row )
{
int col = 0;
elements[ col + row * 4 ] = ( lhs[ col, 0 ] * rhs[ 0, row ] ) + ( lhs[ col, 1 ] * rhs[ 1, row ] ) + ( lhs[ col, 2 ] * rhs[ 2, row ] ) + ( lhs[ col, 3 ] * rhs[ 3, row ] ); ++col;
elements[ col + row * 4 ] = ( lhs[ col, 0 ] * rhs[ 0, row ] ) + ( lhs[ col, 1 ] * rhs[ 1, row ] ) + ( lhs[ col, 2 ] * rhs[ 2, row ] ) + ( lhs[ col, 3 ] * rhs[ 3, row ] ); ++col;
elements[ col + row * 4 ] = ( lhs[ col, 0 ] * rhs[ 0, row ] ) + ( lhs[ col, 1 ] * rhs[ 1, row ] ) + ( lhs[ col, 2 ] * rhs[ 2, row ] ) + ( lhs[ col, 3 ] * rhs[ 3, row ] ); ++col;
elements[ col + row * 4 ] = ( lhs[ col, 0 ] * rhs[ 0, row ] ) + ( lhs[ col, 1 ] * rhs[ 1, row ] ) + ( lhs[ col, 2 ] * rhs[ 2, row ] ) + ( lhs[ col, 3 ] * rhs[ 3, row ] ); ++col;
}
}
/// <summary> /// Applies the specified transform, adds it to the specified transform stack /// </summary> public abstract void SetTransform( TransformType type, InvariantMatrix44 matrix );
/// <summary>
/// Makes a translation matrix from a vector
/// </summary>
public static InvariantMatrix44 MakeTranslationMatrix( float x, float y, float z )
{
InvariantMatrix44 matrix = new InvariantMatrix44( );
MakeTranslationMatrix( matrix, x, y, z );
return matrix;
}
/// <summary>
/// Makes the specified matrix into a translation matrix
/// </summary>
protected static void MakeTranslationMatrix( InvariantMatrix44 matrix, float x, float y, float z )
{
float[] elements = matrix.Elements;
elements[ 0 ] = 1; elements[ 1 ] = 0; elements[ 2 ] = 0; elements[ 3 ] = 0;
elements[ 4 ] = 0; elements[ 5 ] = 1; elements[ 6 ] = 0; elements[ 7 ] = 0;
elements[ 8 ] = 0; elements[ 9 ] = 0; elements[ 10 ] = 1; elements[ 11 ] = 0;
elements[ 12 ] = x; elements[ 13 ] = y; elements[ 14 ] = z; elements[ 15 ] = 1;
}
/// <summary>
/// Stores the reflection of src in result
/// </summary>
public static InvariantMatrix44 MakeReflectionMatrix( Point3 pointOnPlane, Vector3 planeNormal )
{
InvariantMatrix44 result = new InvariantMatrix44( );
MakeReflectionMatrix( result, pointOnPlane, planeNormal );
return result;
}
/// <summary>
/// Makes a matrix storing a rotation around the z-axis
/// </summary>
public static InvariantMatrix44 MakeRotationAroundZAxisMatrix( float angleInRadians )
{
InvariantMatrix44 matrix = new InvariantMatrix44( );
MakeRotationAroundZAxisMatrix( matrix, angleInRadians );
return matrix;
}
/// <summary>
/// Makes a matrix from a quaternion
/// </summary>
public static InvariantMatrix44 MakeQuaternionMatrix( Quaternion quaternion )
{
InvariantMatrix44 result = new InvariantMatrix44( );
MakeQuaternionMatrix( result, quaternion );
return result;
}
/// <summary>
/// Returns the multiple of lhs and rhs
/// </summary>
public static InvariantMatrix44 operator *( InvariantMatrix44 lhs, InvariantMatrix44 rhs )
{
InvariantMatrix44 result = new InvariantMatrix44( );
Multiply( result, lhs, rhs );
return result;
}
/// <summary>
/// Helper to convert a Matrix44 to a GL-friendly float array
/// </summary>
private static float[] GetGlMatrix( InvariantMatrix44 matrix )
{
return matrix is Matrix44 ? ( ( Matrix44 )matrix ).Elements : matrix.ToFloatArray( );
}
/// <summary>
/// Creates a matrix from a planar reflection
/// </summary>
protected static void MakeReflectionMatrix( InvariantMatrix44 matrix, Point3 pointOnPlane, Vector3 planeNormal )
{
float[] elements = matrix.Elements;
float np2 = 2.0f * planeNormal.Dot( pointOnPlane );
elements[ 0 ] = 1.0f - 2.0f * planeNormal.X * planeNormal.X;
elements[ 4 ] = -2.0f * planeNormal.X * planeNormal.Y;
elements[ 8 ] = -2.0f * planeNormal.X * planeNormal.Z;
elements[ 12 ] = np2 * planeNormal.X;
elements[ 1 ] = -2.0f * planeNormal.Y * planeNormal.X;
elements[ 5 ] = 1.0f - 2.0f * planeNormal.Y * planeNormal.Y;
elements[ 9 ] = -2.0f * planeNormal.Y * planeNormal.Z;
elements[ 13 ] = np2 * planeNormal.Y;
elements[ 2 ] = -2.0f * planeNormal.Z * planeNormal.X;
elements[ 6 ] = -2.0f * planeNormal.Z * planeNormal.Y;
elements[ 10 ] = 1.0f - 2.0f * planeNormal.Z * planeNormal.Z;
elements[ 14 ] = np2 * planeNormal.Z;
elements[ 3 ] = 0.0f;
elements[ 7 ] = 0.0f;
elements[ 11 ] = 0.0f;
elements[ 15 ] = 1.0f;
}
/// <summary>
/// Creates an inverted copy of this matrix
/// </summary>
public InvariantMatrix44 Invert( )
{
InvariantMatrix44 result = new InvariantMatrix44( );
MakeInverse( result, this );
return result;
}
/// <summary>
/// Makes a matrix storing a rotation around the z-axis
/// </summary>
protected static void MakeRotationAroundZAxisMatrix( InvariantMatrix44 matrix, float angleInRadians )
{
float sinA = Functions.Sin( angleInRadians );
float cosA = Functions.Cos( angleInRadians );
float[] elements = matrix.Elements;
elements[ 0 ] = cosA; elements[ 1 ] = sinA; elements[ 2 ] = 0; elements[ 3 ] = 0;
elements[ 4 ] = -sinA; elements[ 5 ] = cosA; elements[ 6 ] = 0; elements[ 7 ] = 0;
elements[ 8 ] = 0; elements[ 9 ] = 0; elements[ 10 ] = 1; elements[ 11 ] = 0;
elements[ 12 ] = 0; elements[ 13 ] = 0; elements[ 14 ] = 0; elements[ 15 ] = 1;
}
/// <summary>
/// Returns true if the two matrices are equal, within a given tolerance per element
/// </summary>
public bool IsCloseTo( InvariantMatrix44 mat, float tol )
{
for ( int index = 0; index < 16; ++index )
{
if ( Math.Abs( this[ index ] - mat[ index ] ) > tol )
{
return false;
}
}
return true;
}
/// <summary>
/// Stores the transpose of src in result
/// </summary>
protected static void MakeTransposeMatrix( InvariantMatrix44 result, InvariantMatrix44 src )
{
for ( int row = 0; row < 4; ++row )
{
for ( int col = 0; col < 4; ++col )
{
result.m_Elements[ row + col * 4 ] = src[ col, row ];
}
}
}
/// <summary>
/// Copies a matrix
/// </summary>
public void Copy( InvariantMatrix44 src )
{
for ( int index = 0; index < 16; ++index )
{
Elements[ index ] = src[ index ];
}
}
/// <summary> /// Pushes a modifier that is applied to any matrix pushed onto the stack /// </summary> /// <remarks> /// Modifiers are only applied to the topmost matrix. For example, pushing a modifier matrix M, /// then pushing world matrices T0, T1 and T2, will result in T0.T1.T2.M being used for rendering. /// </remarks> public abstract void PushTransformPostModifier( TransformType type, InvariantMatrix44 matrix );
/// <summary>
/// Applies the specified transform, multiplied by the current topmost transform, and adds it to the specified transform stack
/// </summary>
public override void PushTransform( TransformType type, InvariantMatrix44 matrix )
{
switch ( type )
{
case TransformType.LocalToWorld :
{
Matrix44 lastLocalToWorld = CurrentLocalToWorld;
++m_TopOfLocalToWorldStack;
CurrentLocalToWorld.StoreMultiply( lastLocalToWorld, matrix );
UpdateModelView( );
break;
}
case TransformType.WorldToView :
{
Matrix44 lastWorldToView = CurrentWorldToView;
++m_TopOfWorldToViewStack;
CurrentWorldToView.StoreMultiply( lastWorldToView, matrix );
UpdateModelView( );
// Matrix44 tmp = new Matrix44( );
// Gl.glGetFloatv( Gl.GL_MODELVIEW_MATRIX, tmp.Elements );
break;
}
default :
{
SetSupportedMatrixMode( type );
Gl.glPushMatrix( );
Gl.glMultMatrixf( GetGlMatrix( matrix ) );
break;
}
}
}
/// <summary>
/// Copies the specified source matrix
/// </summary>
public Matrix44( InvariantMatrix44 src )
: base(src)
{
}
/// <summary>
/// Pushes a modifier that is applied to any matrix pushed onto the stack
/// </summary>
/// <remarks>
/// Modifiers are only applied to the topmost matrix. For example, pushing a modifier matrix M,
/// then pushing world matrices T0, T1 and T2, will result in T0.T1.T2.M being used for rendering.
/// </remarks>
public override void PushTransformPostModifier( TransformType type, InvariantMatrix44 matrix )
{
switch ( type )
{
case TransformType.LocalToWorld :
{
m_LocalToWorldModifierStack[ ++m_TopOfLocalToWorldModifierStack ] = matrix;
break;
}
case TransformType.WorldToView :
{
m_WorldToViewModifierStack[ ++m_TopOfWorldToViewModifierStack ] = matrix;
break;
}
default :
throw new NotSupportedException( string.Format( "Transform type \"{0}\" does not support a modifier matrix", type ) );
}
}
/// <summary>
/// Stores the result of multiplying lhs * rhs in this matrix
/// </summary>
public void StoreMultiply( InvariantMatrix44 lhs, InvariantMatrix44 rhs )
{
Multiply( this, lhs, rhs );
}
/// <summary>
/// Creates a transposed copy of this matrix
/// </summary>
public InvariantMatrix44 Transpose( )
{
InvariantMatrix44 result = new InvariantMatrix44( );
MakeTransposeMatrix( result, this );
return result;
}
