// void timer1_Tick(object sender, System.EventArgs e) {
// throw new System.NotImplementedException();
// }
//////////////////////////////////////////////////////////////////////////
// Test methods compile
static void TestFloat3x3()
{
if ( System.Runtime.InteropServices.Marshal.SizeOf(typeof(float3x3)) != 36 )
throw new Exception( "Not the appropriate size!" );
float3x3 test1 = new float3x3( new float[9] { 9, 8, 7, 6, 5, 4, 3, 2, 1 } );
float3x3 test2 = new float3x3( new float3( 1, 0, 0 ), new float3( 0, 1, 0 ), new float3( 0, 0, 1 ) );
float3x3 test3 = new float3x3( 0, 1, 2, 3, 4, 5, 6, 7, 8 );
float3x3 test0;
test0 = test3;
test2.Scale( new float3( 2, 2, 2 ) );
float3x3 mul0 = test1 * test2;
float3x3 mul1 = 3.0f * test2;
float3 mul2 = new float3( 1, 1, 1 ) * test3;
float3 access0 = test3[2];
test3[1] = new float3( 12, 13, 14 );
float access1 = test3[1,2];
test3[1,2] = 18;
// float coFactor = test3.CoFactor( 0, 2 );
float det = test3.Determinant;
float3x3 inv = test2.Inverse;
float3x3 id = float3x3.Identity;
test3.BuildRotationX( 0.5f );
test3.BuildRotationY( 0.5f );
test3.BuildRotationZ( 0.5f );
test3.BuildFromAngleAxis( 0.5f, float3.UnitY );
}
/// <summary>
/// Builds the matrix to apply the SH coefficients to make them rotate by the specified matrix
/// </summary>
/// <param name="_Rotation">The 3x3 rotation matrix to infer the SH rotation matrix from</param>
/// <param name="_Matrix">The resulting _Order² x _Order² rotation matrix</param>
/// <param name="_Order">The order of the SH vectors that will be rotated using the matrix</param>
public static void BuildRotationMatrix( float3x3 _Rotation, double[,] _Matrix, int _Order )
{
// This method recursively computes the SH transformation matrix based on a regular 3x3 transform
//
// The transformation matrix is block diagonal-sparse in the following form:
//
// 1 | 0 | 0 | 0 | 0 | ...
// ---------------------------------------------
// 0 | m00 | m01 | m02 | 0 | ...
// ---------------------------------------------
// 0 | m10 | m11 | m12 | 0 | ...
// ---------------------------------------------
// 0 | m20 | m21 | m22 | 0 | ...
// ---------------------------------------------
// 0 | 0 | 0 | 0 | xxx | ...
// ---------------------------------------------
// ... | ... | ... | ... | ... | ...
//
// The blocks from order > 1 are infered by reccurence from the block at order 1, which is a regular 3x3 matrix.
//
// Clear coefficients
for ( int DimIndex0=0; DimIndex0 < _Matrix.GetLength( 0 ); DimIndex0++ )
for ( int DimIndex1=0; DimIndex1 < _Matrix.GetLength( 1 ); DimIndex1++ )
_Matrix[DimIndex0,DimIndex0] = 0.0;
// Order 0 is the ambient term (constant)
_Matrix[0,0] = 1.0f;
// Order 1 is a regular 3x3 rotation matrix
_Matrix[1,1] = _Rotation[0,0]; _Matrix[1,2] = _Rotation[0,1]; _Matrix[1,3] = _Rotation[0,2];
_Matrix[2,1] = _Rotation[1,0]; _Matrix[2,2] = _Rotation[1,1]; _Matrix[2,3] = _Rotation[1,2];
_Matrix[3,1] = _Rotation[2,0]; _Matrix[3,2] = _Rotation[2,1]; _Matrix[3,3] = _Rotation[2,2];
// The remaining bands are built recursively
for ( int l=2; l < _Order; l++ )
{
int BandOffset = l * (l + 1);
for ( int m=-l; m <= +l; m++ )
{
for ( int n=-l; n <= +l; n++ )
{
// Compute (u, v, w)
double u, v, w;
double m0 = m == 0 ? 1.0 : 0.0;
double m1 = m == 1 ? 1.0 : 0.0;
double mm1 = m == -1 ? 1.0 : 0.0;
if ( Math.Abs(n) < l )
{
u = Math.Sqrt( (l + m) * (l - m) / ((l + n) * (l - n)) );
v = +.5 * Math.Sqrt( (1.0 + m0) * (l + Math.Abs( m ) - 1) * (l + Math.Abs( m )) / ((l + n) * (l - n)) ) * (1.0 - 2.0 * m0);
w = -.5 * Math.Sqrt( (l - Math.Abs( m ) - 1) * (l - Math.Abs( m )) / ((l + n) * (l - n)) ) * (1.0 - m0);
}
else
{
u = Math.Sqrt( (l + m) * (l - m) / (2 * l * (2 * l - 1)) );
v = +.5 * Math.Sqrt( (1.0 + m0) * (l + Math.Abs( m ) - 1) * (l + Math.Abs( m )) / (2 * l * (2 * l - 1)) ) * (1.0 - 2.0 * m0);
w = -.5 * Math.Sqrt( (l - Math.Abs( m ) - 1) * (l - Math.Abs( m )) / (2 * l * (2 * l - 1)) ) * (1.0 - m0);
}
// Compute (U, V, W)
double U, V, W;
if ( m > 0 )
{
U = ComputeP( m, n, 0, l, _Matrix );
V = ComputeP( m - 1, n, 1, l, _Matrix ) * Math.Sqrt( 1.0 + m1 ) - ComputeP( 1 - m, n, -1, l, _Matrix ) * (1.0 - m1);
W = ComputeP( m + 1, n, 1, l, _Matrix ) + ComputeP( -m - 1, n, -1, l, _Matrix );
}
else if ( m < 0 )
{
U = ComputeP( m, n, 0, l, _Matrix );
V = ComputeP( m + 1, n, 1, l, _Matrix ) * (1.0 + mm1) + ComputeP( -m - 1, n, -1, l, _Matrix ) * Math.Sqrt( 1.0 - mm1 );
W = ComputeP( m - 1, n, 1, l, _Matrix ) - ComputeP( -m + 1, n, -1, l, _Matrix );
}
else
{
U = ComputeP( 0, n, 0, l, _Matrix );
V = ComputeP( 1, n, 1, l, _Matrix ) + ComputeP( -1, n, -1, l, _Matrix );
W = 0.0;
}
// Compute final coefficient
_Matrix[BandOffset + m,BandOffset + n] = u * U + v * V + w * W;
}
}
}
}
public void FromEuler(float3 _EulerAngles)
{
float3x3 MatX = new float3x3(INIT_TYPES.ROT_X, _EulerAngles.x); float3x3 MatY = new float3x3(INIT_TYPES.ROT_Y, _EulerAngles.y); float3x3 MatZ = new float3x3(INIT_TYPES.ROT_Z, _EulerAngles.z); Set(MatX * MatY * MatZ);
}
public float3x3(float3x3 _Source)
{
Set(_Source);
}
public void Set( float3x3 _Source )
{
if ( _Source == null )
return;
m[0, 0] = _Source.m[0, 0]; m[0, 1] = _Source.m[0, 1]; m[0, 2] = _Source.m[0, 2];
m[1, 0] = _Source.m[1, 0]; m[1, 1] = _Source.m[1, 1]; m[1, 2] = _Source.m[1, 2];
m[2, 0] = _Source.m[2, 0]; m[2, 1] = _Source.m[2, 1]; m[2, 2] = _Source.m[2, 2];
}
public float3x3 MakePYR(float _fPitch, float _fYaw, float _fRoll)
{
float3x3 Pitch = new float3x3(INIT_TYPES.ROT_X, _fPitch); float3x3 Yaw = new float3x3(INIT_TYPES.ROT_Y, _fYaw); float3x3 Roll = new float3x3(INIT_TYPES.ROT_Z, _fRoll); Set(Roll * Yaw * Pitch); return(this);
}
public float3x3 Invert()
{
float fDet = Determinant();
if ( (float) System.Math.Abs(fDet) < float.Epsilon )
throw new MatrixException( "Matrix is not invertible!" ); // The matrix is not invertible! Singular case!
float fIDet = 1.0f / fDet;
float3x3 Temp = new float3x3();
Temp.m[0, 0] = +(m[1, 1] * m[2, 2] - m[2, 1] * m[1, 2]) * fIDet;
Temp.m[1, 0] = -(m[1, 0] * m[2, 2] - m[2, 0] * m[1, 2]) * fIDet;
Temp.m[2, 0] = +(m[1, 0] * m[2, 1] - m[2, 0] * m[1, 1]) * fIDet;
Temp.m[0, 1] = -(m[0, 1] * m[2, 2] - m[2, 1] * m[0, 2]) * fIDet;
Temp.m[1, 1] = +(m[0, 0] * m[2, 2] - m[2, 0] * m[0, 2]) * fIDet;
Temp.m[2, 1] = -(m[0, 0] * m[2, 1] - m[2, 0] * m[0, 1]) * fIDet;
Temp.m[0, 2] = +(m[0, 1] * m[1, 2] - m[1, 1] * m[0, 2]) * fIDet;
Temp.m[1, 2] = -(m[0, 0] * m[1, 2] - m[1, 0] * m[0, 2]) * fIDet;
Temp.m[2, 2] = +(m[0, 0] * m[1, 1] - m[1, 0] * m[0, 1]) * fIDet;
Set( Temp );
return this;
}
public float3x3 MakePYR( float _fPitch, float _fYaw, float _fRoll )
{
float3x3 Pitch = new float3x3( INIT_TYPES.ROT_X, _fPitch ); float3x3 Yaw = new float3x3( INIT_TYPES.ROT_Y, _fYaw ); float3x3 Roll = new float3x3( INIT_TYPES.ROT_Z, _fRoll ); Set( Roll * Yaw * Pitch ); return this;
}
public static float3x3 Parse( string _Source )
{
string[] Terms = _Source.Split( new char[] { ';' } );
float3x3 Result = new float3x3();
Result.m[0,0] = float.Parse( Terms[3 * 0 + 0] );
Result.m[0,1] = float.Parse( Terms[3 * 0 + 1] );
Result.m[0,2] = float.Parse( Terms[3 * 0 + 2] );
Result.m[1,0] = float.Parse( Terms[3 * 1 + 0] );
Result.m[1,1] = float.Parse( Terms[3 * 1 + 1] );
Result.m[1,2] = float.Parse( Terms[3 * 1 + 2] );
Result.m[2,0] = float.Parse( Terms[3 * 2 + 0] );
Result.m[2,1] = float.Parse( Terms[3 * 2 + 1] );
Result.m[2,2] = float.Parse( Terms[3 * 2 + 2] );
return Result;
}
public void FromEuler( float3 _EulerAngles )
{
float3x3 MatX = new float3x3( INIT_TYPES.ROT_X, _EulerAngles.x ); float3x3 MatY = new float3x3( INIT_TYPES.ROT_Y, _EulerAngles.y ); float3x3 MatZ = new float3x3( INIT_TYPES.ROT_Z, _EulerAngles.z ); Set( MatX * MatY * MatZ );
}
public float3x3( float3x3 _Source )
{
Set( _Source );
}
/// <summary>
/// Computes the rotation matrix to transform a source vector into a target vector
/// (routine from Thomas Moller)
/// </summary>
/// <param name="_Source">The source vector</param>
/// <param name="_Target">The target vector</param>
/// <returns>The rotation matrix to apply that will transform</returns>
public static float3x3 ComputeRotationMatrix( float3 _Source, float3 _Target )
{
SharpMath.float3x3 Result = new float3x3();
Result.MakeIdentity();
float e = _Source | _Target;
bool bReverse = e < 0.0f;
if ( bReverse )
{ // Revert target
_Target = -_Target;
e = -e;
}
if ( e > 1.0f - 0.000001f )
{
if ( bReverse )
{ // Reverse final matrix
Result.SetRow0( -Result.GetRow0() );
Result.SetRow1( -Result.GetRow1() );
Result.SetRow2( -Result.GetRow2() );
}
return Result; // No rotation needed...
}
float3 Ortho = _Source ^ _Target;
float h = 1.0f / (1.0f + e); // Optimization by Gottfried Chen
Result.SetRow0( new float3( e + h * Ortho.x * Ortho.x,
h * Ortho.x * Ortho.y + Ortho.z,
h * Ortho.x * Ortho.z - Ortho.y ) );
Result.SetRow1( new float3( h * Ortho.x * Ortho.y - Ortho.z,
e + h * Ortho.y * Ortho.y,
h * Ortho.y * Ortho.z + Ortho.x ) );
Result.SetRow2( new float3( h * Ortho.x * Ortho.z + Ortho.y,
h * Ortho.y * Ortho.z - Ortho.x,
e + h * Ortho.z * Ortho.z ) );
if ( bReverse )
{ // Reverse final matrix
Result.SetRow0( -Result.GetRow0() );
Result.SetRow1( -Result.GetRow1() );
Result.SetRow2( -Result.GetRow2() );
}
return Result;
}
public static float3x3 operator /( float3x3 _Op0, float _s )
{
float Is = 1.0f / _s;
float3x3 Ret = new float3x3();
Ret.m[0, 0] = _Op0.m[0, 0] * Is;
Ret.m[0, 1] = _Op0.m[0, 1] * Is;
Ret.m[0, 2] = _Op0.m[0, 2] * Is;
Ret.m[1, 0] = _Op0.m[1, 0] * Is;
Ret.m[1, 1] = _Op0.m[1, 1] * Is;
Ret.m[1, 2] = _Op0.m[1, 2] * Is;
Ret.m[2, 0] = _Op0.m[2, 0] * Is;
Ret.m[2, 1] = _Op0.m[2, 1] * Is;
Ret.m[2, 2] = _Op0.m[2, 2] * Is;
return Ret;
}
public static float3x3 operator -( float3x3 _Op0, float3x3 _Op1 )
{
float3x3 Ret = new float3x3();
Ret.m[0, 0] = _Op0.m[0, 0] - _Op1.m[0, 0];
Ret.m[0, 1] = _Op0.m[0, 1] - _Op1.m[0, 1];
Ret.m[0, 2] = _Op0.m[0, 2] - _Op1.m[0, 2];
Ret.m[1, 0] = _Op0.m[1, 0] - _Op1.m[1, 0];
Ret.m[1, 1] = _Op0.m[1, 1] - _Op1.m[1, 1];
Ret.m[1, 2] = _Op0.m[1, 2] - _Op1.m[1, 2];
Ret.m[2, 0] = _Op0.m[2, 0] - _Op1.m[2, 0];
Ret.m[2, 1] = _Op0.m[2, 1] - _Op1.m[2, 1];
Ret.m[2, 2] = _Op0.m[2, 2] - _Op1.m[2, 2];
return Ret;
}
public static float3x3 operator *( float _s, float3x3 _Op0 )
{
float3x3 Ret = new float3x3();
Ret.m[0, 0] = _Op0.m[0, 0] * _s;
Ret.m[0, 1] = _Op0.m[0, 1] * _s;
Ret.m[0, 2] = _Op0.m[0, 2] * _s;
Ret.m[1, 0] = _Op0.m[1, 0] * _s;
Ret.m[1, 1] = _Op0.m[1, 1] * _s;
Ret.m[1, 2] = _Op0.m[1, 2] * _s;
Ret.m[2, 0] = _Op0.m[2, 0] * _s;
Ret.m[2, 1] = _Op0.m[2, 1] * _s;
Ret.m[2, 2] = _Op0.m[2, 2] * _s;
return Ret;
}
