public void ToArrayTest( )
{
Matrix4x4 matrix = new Matrix4x4( );
matrix.V00 = 1;
matrix.V01 = 2;
matrix.V02 = 3;
matrix.V03 = 4;
matrix.V10 = 5;
matrix.V11 = 6;
matrix.V12 = 7;
matrix.V13 = 8;
matrix.V20 = 9;
matrix.V21 = 10;
matrix.V22 = 11;
matrix.V23 = 12;
matrix.V30 = 13;
matrix.V31 = 14;
matrix.V32 = 15;
matrix.V33 = 16;
float[] array = matrix.ToArray( );
for ( int i = 0; i < 16; i++ )
{
Assert.AreEqual<float>( array[i], (float) ( i + 1 ) );
}
}
public void updateAngle(float y,float p,float r)
{
worldMatrix =
Matrix4x4.CreateTranslation(new Vector3(0, 0, 0)) *
Matrix4x4.CreateFromYawPitchRoll(y, p, r);
Recalculate();
}
public void AddMatricesTest( )
{
Matrix4x4 expectedResult = new Matrix4x4( );
expectedResult.V00 = 3;
expectedResult.V01 = 3;
expectedResult.V02 = 7;
expectedResult.V03 = 7;
expectedResult.V10 = 8;
expectedResult.V11 = 5;
expectedResult.V12 = 5;
expectedResult.V13 = 2;
expectedResult.V20 = 6;
expectedResult.V21 = 5;
expectedResult.V22 = 5;
expectedResult.V23 = 4;
expectedResult.V30 = 3;
expectedResult.V31 = 7;
expectedResult.V32 = 3;
expectedResult.V33 = 7;
Matrix4x4 result = a1 + a2;
Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) );
}
public void setTransformatinMatrix(Matrix4x4 m)
{
estimate00.Text = m.V00.ToString();
estimate10.Text = m.V01.ToString();
estimate20.Text = m.V01.ToString();
estimate30.Text = m.V03.ToString();
estimate01.Text = m.V10.ToString();
estimate11.Text = m.V11.ToString();
estimate21.Text = m.V12.ToString();
estimate31.Text = m.V13.ToString();
estimate02.Text = m.V20.ToString();
estimate12.Text = m.V21.ToString();
estimate22.Text = m.V22.ToString();
estimate32.Text = m.V23.ToString();
estimate03.Text = m.V30.ToString();
estimate13.Text = m.V31.ToString();
estimate23.Text = m.V32.ToString();
estimate33.Text = m.V33.ToString();
}
public void SetMatrix( Matrix4x4 matrix )
{
float[] array = matrix.ToArray( );
for ( int i = 0, k = 0; i < 4; i++ )
{
for ( int j = 0; j < 4; j++, k++ )
{
string textBoxName = string.Format( "i{0}_j{1}_Box", i, j );
if ( textBoxes.ContainsKey( textBoxName ) )
{
textBoxes[textBoxName].Text = FormatFloat( array[k] );
}
}
}
}
/// <summary>
/// Executes the transform calculations (T = Z*X).
/// </summary>
///
/// <returns>Transform matrix T.</returns>
///
/// <remarks>Calling this method also updates the Transform property.</remarks>
///
public void Compute(DenavitHartenbergParameters parameters)
{
// Calculate Z with Z = TranslationZ(d).RotationZ(theta)
Z = Matrix4x4.Multiply
(
Matrix4x4.CreateTranslation(new Vector3(0f, 0f, (float)parameters.Offset)),
Matrix4x4.CreateRotationZ((float)parameters.Theta)
);
// Calculate X with X = TranslationX(radius).RotationZ(alpha)
X = Matrix4x4.Multiply
(
Matrix4x4.CreateTranslation(new Vector3((float)parameters.Radius, 0f, 0f)),
Matrix4x4.CreateRotationX((float)parameters.Alpha)
);
// Calculate the transform with T=Z.X
Transform = Matrix4x4.Multiply(Z, X);
}
public void setTransformatinMatrix(Matrix4x4 m)
{
est00.Text = m.V00.ToString();
est01.Text = m.V01.ToString();
est02.Text = m.V01.ToString();
est03.Text = m.V03.ToString();
est10.Text = m.V10.ToString();
est11.Text = m.V11.ToString();
est12.Text = m.V12.ToString();
est13.Text = m.V13.ToString();
est20.Text = m.V20.ToString();
est21.Text = m.V21.ToString();
est22.Text = m.V22.ToString();
est23.Text = m.V23.ToString();
est30.Text = m.V30.ToString();
est31.Text = m.V31.ToString();
est32.Text = m.V32.ToString();
est33.Text = m.V33.ToString();
}
public ViewPosit(int w,int h)
{
sizeRect = 50;
wImg = w;
hImg = h;
center = new Point(w / 2, h / 2);
pktList = new System.Collections.Generic.List<Point>();
pktList.Add(new Point(center.X + sizeRect, center.Y + sizeRect));
pktList.Add(new Point(center.X - sizeRect, center.Y + sizeRect));
pktList.Add(new Point(center.X - sizeRect, center.Y - sizeRect));
pktList.Add(new Point(center.X + sizeRect, center.Y - sizeRect));
colorList = new System.Collections.Generic.List<Bgr>();
colorList.Add(new Bgr(255,0,0));
colorList.Add(new Bgr(0,255,0));
colorList.Add(new Bgr(0,0,255));
colorList.Add(new Bgr(255,0,255));
worldMatrix = Matrix4x4.Identity;
viewMatrix = Matrix4x4.CreateLookAt(new Vector3(0, 0, 5), new Vector3(0, 0, 0));
perspectiveMatrix = Matrix4x4.CreatePerspective(1, 1, 1, 1000);
}
public void setMtx(Matrix4x4 mtx)
{
m00.Text = mtx.V00.ToString();
m01.Text = mtx.V01.ToString();
m02.Text = mtx.V02.ToString();
m03.Text = mtx.V03.ToString();
m10.Text = mtx.V10.ToString();
m11.Text = mtx.V11.ToString();
m12.Text = mtx.V12.ToString();
m13.Text = mtx.V13.ToString();
m20.Text = mtx.V20.ToString();
m21.Text = mtx.V21.ToString();
m22.Text = mtx.V22.ToString();
m23.Text = mtx.V23.ToString();
m30.Text = mtx.V30.ToString();
m31.Text = mtx.V31.ToString();
m32.Text = mtx.V32.ToString();
m33.Text = mtx.V33.ToString();
}
public WorldRendererControl( )
{
InitializeComponent( );
objectPoints = new Vector3[]
{
new Vector3( 0, 0, 0 ),
};
colors = new Color[]
{
Color.White,
};
lines = new int[0, 2];
// create default matrices
worldMatrix = Matrix4x4.Identity;
viewMatrix = Matrix4x4.CreateLookAt( new Vector3( 0, 0, 5 ), new Vector3( 0, 0, 0 ) );
perspectiveMatrix = Matrix4x4.CreatePerspective( 1, 1, 1, 1000 );
Recalculate( );
}
/// <summary>
/// Adds corresponding components of two matrices.
/// </summary>
///
/// <param name="matrix1">The matrix to add to.</param>
/// <param name="matrix2">The matrix to add to the first matrix.</param>
///
/// <returns>Returns a matrix which components are equal to sum of corresponding
/// components of the two specified matrices.</returns>
///
public static Matrix4x4 Add(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 + matrix2;
}
/// <summary>
/// Adds corresponding components of two matrices.
/// </summary>
///
/// <param name="matrix1">The matrix to add to.</param>
/// <param name="matrix2">The matrix to add to the first matrix.</param>
///
/// <returns>Returns a matrix which components are equal to sum of corresponding
/// components of the two specified matrices.</returns>
///
public static Matrix4x4 operator +(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = matrix1.V00 + matrix2.V00;
m.V01 = matrix1.V01 + matrix2.V01;
m.V02 = matrix1.V02 + matrix2.V02;
m.V03 = matrix1.V03 + matrix2.V03;
m.V10 = matrix1.V10 + matrix2.V10;
m.V11 = matrix1.V11 + matrix2.V11;
m.V12 = matrix1.V12 + matrix2.V12;
m.V13 = matrix1.V13 + matrix2.V13;
m.V20 = matrix1.V20 + matrix2.V20;
m.V21 = matrix1.V21 + matrix2.V21;
m.V22 = matrix1.V22 + matrix2.V22;
m.V23 = matrix1.V23 + matrix2.V23;
m.V30 = matrix1.V30 + matrix2.V30;
m.V31 = matrix1.V31 + matrix2.V31;
m.V32 = matrix1.V32 + matrix2.V32;
m.V33 = matrix1.V33 + matrix2.V33;
return m;
}
/// <summary>
/// Multiplies two specified matrices.
/// </summary>
///
/// <param name="matrix1">Matrix to multiply.</param>
/// <param name="matrix2">Matrix to multiply by.</param>
///
/// <returns>Return new matrix, which the result of multiplication of the two specified matrices.</returns>
///
public static Matrix4x4 Multiply(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 * matrix2;
}
/// <summary>
/// Multiplies two specified matrices.
/// </summary>
///
/// <param name="matrix1">Matrix to multiply.</param>
/// <param name="matrix2">Matrix to multiply by.</param>
///
/// <returns>Return new matrix, which the result of multiplication of the two specified matrices.</returns>
///
public static Matrix4x4 operator *(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = matrix1.V00 * matrix2.V00 + matrix1.V01 * matrix2.V10 + matrix1.V02 * matrix2.V20 + matrix1.V03 * matrix2.V30;
m.V01 = matrix1.V00 * matrix2.V01 + matrix1.V01 * matrix2.V11 + matrix1.V02 * matrix2.V21 + matrix1.V03 * matrix2.V31;
m.V02 = matrix1.V00 * matrix2.V02 + matrix1.V01 * matrix2.V12 + matrix1.V02 * matrix2.V22 + matrix1.V03 * matrix2.V32;
m.V03 = matrix1.V00 * matrix2.V03 + matrix1.V01 * matrix2.V13 + matrix1.V02 * matrix2.V23 + matrix1.V03 * matrix2.V33;
m.V10 = matrix1.V10 * matrix2.V00 + matrix1.V11 * matrix2.V10 + matrix1.V12 * matrix2.V20 + matrix1.V13 * matrix2.V30;
m.V11 = matrix1.V10 * matrix2.V01 + matrix1.V11 * matrix2.V11 + matrix1.V12 * matrix2.V21 + matrix1.V13 * matrix2.V31;
m.V12 = matrix1.V10 * matrix2.V02 + matrix1.V11 * matrix2.V12 + matrix1.V12 * matrix2.V22 + matrix1.V13 * matrix2.V32;
m.V13 = matrix1.V10 * matrix2.V03 + matrix1.V11 * matrix2.V13 + matrix1.V12 * matrix2.V23 + matrix1.V13 * matrix2.V33;
m.V20 = matrix1.V20 * matrix2.V00 + matrix1.V21 * matrix2.V10 + matrix1.V22 * matrix2.V20 + matrix1.V23 * matrix2.V30;
m.V21 = matrix1.V20 * matrix2.V01 + matrix1.V21 * matrix2.V11 + matrix1.V22 * matrix2.V21 + matrix1.V23 * matrix2.V31;
m.V22 = matrix1.V20 * matrix2.V02 + matrix1.V21 * matrix2.V12 + matrix1.V22 * matrix2.V22 + matrix1.V23 * matrix2.V32;
m.V23 = matrix1.V20 * matrix2.V03 + matrix1.V21 * matrix2.V13 + matrix1.V22 * matrix2.V23 + matrix1.V23 * matrix2.V33;
m.V30 = matrix1.V30 * matrix2.V00 + matrix1.V31 * matrix2.V10 + matrix1.V32 * matrix2.V20 + matrix1.V33 * matrix2.V30;
m.V31 = matrix1.V30 * matrix2.V01 + matrix1.V31 * matrix2.V11 + matrix1.V32 * matrix2.V21 + matrix1.V33 * matrix2.V31;
m.V32 = matrix1.V30 * matrix2.V02 + matrix1.V31 * matrix2.V12 + matrix1.V32 * matrix2.V22 + matrix1.V33 * matrix2.V32;
m.V33 = matrix1.V30 * matrix2.V03 + matrix1.V31 * matrix2.V13 + matrix1.V32 * matrix2.V23 + matrix1.V33 * matrix2.V33;
return m;
}
public void MultiplyMatricesTest( )
{
Matrix4x4 expectedResult = new Matrix4x4( );
expectedResult.V00 = 23;
expectedResult.V01 = 29;
expectedResult.V02 = 21;
expectedResult.V03 = 27;
expectedResult.V10 = 27;
expectedResult.V11 = 21;
expectedResult.V12 = 29;
expectedResult.V13 = 23;
expectedResult.V20 = 24;
expectedResult.V21 = 27;
expectedResult.V22 = 23;
expectedResult.V23 = 26;
expectedResult.V30 = 26;
expectedResult.V31 = 23;
expectedResult.V32 = 27;
expectedResult.V33 = 24;
Matrix4x4 result = a1 * a2;
Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) );
}
private AForge.Point[] PerformProjection( Vector3[] model, Matrix4x4 transformationMatrix, int viewSize )
{
AForge.Point[] projectedPoints = new AForge.Point[model.Length];
for ( int i = 0; i < model.Length; i++ )
{
Vector3 scenePoint = ( transformationMatrix * model[i].ToVector4( ) ).ToVector3( );
projectedPoints[i] = new AForge.Point(
(int) ( scenePoint.X / scenePoint.Z * viewSize ),
(int) ( scenePoint.Y / scenePoint.Z * viewSize ) );
}
return projectedPoints;
}
/// <summary>
/// Multiplies specified matrix by the specified vector.
/// </summary>
///
/// <param name="matrix">Matrix to multiply by vector.</param>
/// <param name="vector">Vector to multiply matrix by.</param>
///
/// <returns>Returns new vector which is the result of multiplication of the specified matrix
/// by the specified vector.</returns>
///
public static Vector4 Multiply(Matrix4x4 matrix, Vector4 vector)
{
return matrix * vector;
}
/// <summary>
/// Creates a perspective projection matrix.
/// </summary>
///
/// <param name="width">Width of the view volume at the near view plane.</param>
/// <param name="height">Height of the view volume at the near view plane.</param>
/// <param name="nearPlaneDistance">Distance to the near view plane.</param>
/// <param name="farPlaneDistance">Distance to the far view plane.</param>
///
/// <returns>Return a perspective projection matrix.</returns>
///
/// <exception cref="ArgumentOutOfRangeException">Both near and far view planes' distances must be greater than zero.</exception>
/// <exception cref="ArgumentException">Near plane must be closer than the far plane.</exception>
///
public static Matrix4x4 CreatePerspective(float width, float height, float nearPlaneDistance, float farPlaneDistance)
{
if (nearPlaneDistance <= 0)
throw new ArgumentOutOfRangeException("nearPlaneDistance ", "Near plane distance must be greater than zero.");
if (farPlaneDistance <= 0)
throw new ArgumentOutOfRangeException("farPlaneDistance", "Far view plane distance must be greater than zero.");
if (nearPlaneDistance >= farPlaneDistance)
throw new ArgumentException("Near plane must be closer than the far plane.", "farPlaneDistance");
Matrix4x4 m = new Matrix4x4();
m.V00 = 2.0f * nearPlaneDistance / width;
m.V11 = 2.0f * nearPlaneDistance / height;
m.V22 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance);
m.V32 = -1;
m.V23 = (nearPlaneDistance * farPlaneDistance) / (nearPlaneDistance - farPlaneDistance);
return m;
}
// Calculate new world transformation matrix
private void UpdateTransformationMatrix( )
{
float rYaw = (float) ( yaw / 180 * Math.PI );
float rPitch = (float) ( pitch / 180 * Math.PI );
float rRoll = (float) ( roll / 180 * Math.PI );
transformationMatrix =
Matrix4x4.CreateTranslation( new Vector3( xObject, yObject, zObject ) ) *
Matrix4x4.CreateFromYawPitchRoll( rYaw, rPitch, rRoll );
worldRendererControl.WorldMatrix = transformationMatrix;
transformationMatrixControl.SetMatrix( transformationMatrix );
targetTransformationMatrixControl.SetMatrix( viewMatrix * transformationMatrix );
UpdateProjectedPoints( );
EstimatePose( );
}
// Calculate new view transformation matrix
private void UpdateViewMatrix( )
{
viewMatrix = Matrix4x4.CreateLookAt(
new Vector3( xCamera, yCamera, zCamera ),
new Vector3( xLookAt, yLookAt, zLookAt ) );
worldRendererControl.ViewMatrix = viewMatrix;
viewMatrixControl.SetMatrix( viewMatrix );
targetTransformationMatrixControl.SetMatrix( viewMatrix * transformationMatrix );
UpdateProjectedPoints( );
EstimatePose( );
}
private bool ApproximateEquals( Matrix4x4 matrix1, Matrix4x4 matrix2 )
{
// TODO: better algorithm should be put into the framework actually
return (
( System.Math.Abs( matrix1.V00 - matrix2.V00 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V01 - matrix2.V01 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V02 - matrix2.V02 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V03 - matrix2.V03 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V10 - matrix2.V10 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V11 - matrix2.V11 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V12 - matrix2.V12 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V13 - matrix2.V13 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V20 - matrix2.V20 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V21 - matrix2.V21 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V22 - matrix2.V22 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V23 - matrix2.V23 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V30 - matrix2.V30 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V31 - matrix2.V31 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V32 - matrix2.V32 ) <= Epsilon ) &&
( System.Math.Abs( matrix1.V33 - matrix2.V33 ) <= Epsilon )
);
}
private void CompareMatrixWithArray( Matrix4x4 matrix, float[] array )
{
float[] matrixArray = matrix.ToArray( );
for ( int i = 0; i < 16; i++ )
{
Assert.AreEqual<float>( matrixArray[i], array[i] );
}
}
/// <summary>
/// Subtracts corresponding components of two matrices.
/// </summary>
///
/// <param name="matrix1">The matrix to subtract from.</param>
/// <param name="matrix2">The matrix to subtract from the first matrix.</param>
///
/// <returns>Returns a matrix which components are equal to difference of corresponding
/// components of the two specified matrices.</returns>
///
public static Matrix4x4 operator -(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = matrix1.V00 - matrix2.V00;
m.V01 = matrix1.V01 - matrix2.V01;
m.V02 = matrix1.V02 - matrix2.V02;
m.V03 = matrix1.V03 - matrix2.V03;
m.V10 = matrix1.V10 - matrix2.V10;
m.V11 = matrix1.V11 - matrix2.V11;
m.V12 = matrix1.V12 - matrix2.V12;
m.V13 = matrix1.V13 - matrix2.V13;
m.V20 = matrix1.V20 - matrix2.V20;
m.V21 = matrix1.V21 - matrix2.V21;
m.V22 = matrix1.V22 - matrix2.V22;
m.V23 = matrix1.V23 - matrix2.V23;
m.V30 = matrix1.V30 - matrix2.V30;
m.V31 = matrix1.V31 - matrix2.V31;
m.V32 = matrix1.V32 - matrix2.V32;
m.V33 = matrix1.V33 - matrix2.V33;
return m;
}
/// <summary>
/// Creates a matrix from 4 rows specified as vectors.
/// </summary>
///
/// <param name="row0">First row of the matrix to create.</param>
/// <param name="row1">Second row of the matrix to create.</param>
/// <param name="row2">Third row of the matrix to create.</param>
/// <param name="row3">Fourth row of the matrix to create.</param>
///
/// <returns>Returns a matrix from specified rows.</returns>
///
public static Matrix4x4 CreateFromRows(Vector4 row0, Vector4 row1, Vector4 row2, Vector4 row3)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = row0.X;
m.V01 = row0.Y;
m.V02 = row0.Z;
m.V03 = row0.W;
m.V10 = row1.X;
m.V11 = row1.Y;
m.V12 = row1.Z;
m.V13 = row1.W;
m.V20 = row2.X;
m.V21 = row2.Y;
m.V22 = row2.Z;
m.V23 = row2.W;
m.V30 = row3.X;
m.V31 = row3.Y;
m.V32 = row3.Z;
m.V33 = row3.W;
return m;
}
/// <summary>
/// Subtracts corresponding components of two matrices.
/// </summary>
///
/// <param name="matrix1">The matrix to subtract from.</param>
/// <param name="matrix2">The matrix to subtract from the first matrix.</param>
///
/// <returns>Returns a matrix which components are equal to difference of corresponding
/// components of the two specified matrices.</returns>
///
public static Matrix4x4 Subtract(Matrix4x4 matrix1, Matrix4x4 matrix2)
{
return matrix1 - matrix2;
}
/// <summary>
/// Creates a matrix from 4 columns specified as vectors.
/// </summary>
///
/// <param name="column0">First column of the matrix to create.</param>
/// <param name="column1">Second column of the matrix to create.</param>
/// <param name="column2">Third column of the matrix to create.</param>
/// <param name="column3">Fourth column of the matrix to create.</param>
///
/// <returns>Returns a matrix from specified columns.</returns>
///
public static Matrix4x4 CreateFromColumns(Vector4 column0, Vector4 column1, Vector4 column2, Vector4 column3)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = column0.X;
m.V10 = column0.Y;
m.V20 = column0.Z;
m.V30 = column0.W;
m.V01 = column1.X;
m.V11 = column1.Y;
m.V21 = column1.Z;
m.V31 = column1.W;
m.V02 = column2.X;
m.V12 = column2.Y;
m.V22 = column2.Z;
m.V32 = column2.W;
m.V03 = column3.X;
m.V13 = column3.Y;
m.V23 = column3.Z;
m.V33 = column3.W;
return m;
}
/// <summary>
/// Tests whether the matrix equals to the specified one.
/// </summary>
///
/// <param name="matrix">The matrix to test equality with.</param>
///
/// <returns>Returns <see langword="true"/> if the two matrices are equal or <see langword="false"/> otherwise.</returns>
///
public bool Equals(Matrix4x4 matrix)
{
return (this == matrix);
}
/// <summary>
/// Creates a diagonal matrix using the specified vector as diagonal elements.
/// </summary>
///
/// <param name="vector">Vector to use for diagonal elements of the matrix.</param>
///
/// <returns>Returns a diagonal matrix.</returns>
///
public static Matrix4x4 CreateDiagonal(Vector4 vector)
{
Matrix4x4 m = new Matrix4x4();
m.V00 = vector.X;
m.V11 = vector.Y;
m.V22 = vector.Z;
m.V33 = vector.W;
return m;
}
/// <summary>
/// Creates a view matrix for the specified camera position and target point.
/// </summary>
///
/// <param name="cameraPosition">Position of camera.</param>
/// <param name="cameraTarget">Target point towards which camera is pointing.</param>
///
/// <returns>Returns a view matrix.</returns>
///
/// <remarks><para>Camera's "up" vector is supposed to be (0, 1, 0).</para></remarks>
///
public static Matrix4x4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget)
{
Matrix4x4 m = new Matrix4x4();
Vector3 vector = cameraPosition - cameraTarget;
vector.Normalize();
Vector3 vector2 = Vector3.Cross(new Vector3(0, 1, 0), vector);
vector2.Normalize();
Vector3 vector3 = Vector3.Cross(vector, vector2);
m.V00 = vector2.X;
m.V01 = vector2.Y;
m.V02 = vector2.Z;
m.V10 = vector3.X;
m.V11 = vector3.Y;
m.V12 = vector3.Z;
m.V20 = vector.X;
m.V21 = vector.Y;
m.V22 = vector.Z;
m.V03 = -Vector3.Dot(cameraPosition, vector2);
m.V13 = -Vector3.Dot(cameraPosition, vector3);
m.V23 = -Vector3.Dot(cameraPosition, vector);
m.V33 = 1;
return m;
}
public void SubtractMatricesTest( )
{
Matrix4x4 expectedResult = new Matrix4x4( );
expectedResult.V00 = -1;
expectedResult.V01 = 1;
expectedResult.V02 = -1;
expectedResult.V03 = 1;
expectedResult.V10 = 0;
expectedResult.V11 = 1;
expectedResult.V12 = -1;
expectedResult.V13 = 0;
expectedResult.V20 = 0;
expectedResult.V21 = -3;
expectedResult.V22 = 3;
expectedResult.V23 = 0;
expectedResult.V30 = 1;
expectedResult.V31 = 1;
expectedResult.V32 = -1;
expectedResult.V33 = -1;
Matrix4x4 result = a1 - a2;
Assert.AreEqual<bool>( true, ApproximateEquals( result, expectedResult ) );
}