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] );
}
}
}
}
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>
/// 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>
/// 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>
/// 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;
}
private Accord.Point[] PerformProjection(Vector3[] model, Matrix4x4 transformationMatrix, int viewSize)
{
Accord.Point[] projectedPoints = new Accord.Point[model.Length];
for (int i = 0; i < model.Length; i++)
{
Vector3 scenePoint = (transformationMatrix * model[i].ToVector4()).ToVector3();
projectedPoints[i] = new Accord.Point(
(int)(scenePoint.X / scenePoint.Z * viewSize),
(int)(scenePoint.Y / scenePoint.Z * viewSize));
}
return projectedPoints;
}
/// <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>
/// 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>
/// 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;
}
/// <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;
}
// 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( );
}
// 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( );
}
/// <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>
/// 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;
}