public void IsValid() { Matrix44D inValidPose = new Matrix44D(new double[, ] { { 1, 2, 3, 0 }, { 4, 5, 6, 0 }, { 7, 8, 9, 0 }, { 0, 0, 0, 1 }, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); Assert.IsTrue(PoseD.IsValid(Matrix44D.CreateRotationZ(0.3))); inValidPose = new Matrix44D(new double[, ] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 1, 0, 1 }, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); inValidPose = new Matrix44D(new double[, ] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, -1, 0 }, { 0, 1, 0, 1 }, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); }
public void EqualsTest_WhenTwoDifferentMatricesAreCompared_ThenResultIsFalse() { Matrix44D first = new Matrix44D(1.1, 1.2, 1.3, 1.4, 2.1, 2.2, 2.3, 2.4, 3.1, 3.2, 3.3, 3.4, 4.1, 4.2, 4.3, 4.4); Matrix44D second = new Matrix44D(7.1, 8.2, 9.9, 1.4, 2.1, 2.2, 2.3, 2.4, 3.1, 3.2, 3.3, 3.4, 4.1, 4.2, 4.3, 4.4); Assert.False(first.Equals(second)); }
public void CreateRotationTest_WhenParameterOffsetAxisAndAngle_ThenPositionIsRotatedAndTranslated() { var axis = new Vector3D(1, 0, 0); var rot = Matrix44D.CreateRotation(new Position3D(0, 0, 10), axis, 90.0.DegToRad()); var pos = rot * new Position3D(0.0, 0.0, 0.0); Assert.Equal(new Position3D(0.0, 10.0, 10.0), pos); }
public void RotationMatrix44() { double angle = -1.6; Vector3D axis = new Vector3D(1.0, 2.0, -3.0); QuaternionD q = QuaternionD.CreateRotation(axis, angle); Matrix44D m44 = Matrix44D.CreateRotation(axis, angle); Assert.IsTrue(Matrix44D.AreNumericallyEqual(q.ToRotationMatrix44(), m44)); }
public void DeterminanteTest_WhenDeterminanteIsCalled_ThenDeterminanteIsCalculated() { Matrix44D first = new Matrix44D(1.1, 1.2, 1.3, 1.4, 2.1, 2.2, 2.3, 2.4, 3.1, 3.2, 3.3, 3.4, 4.1, 4.2, 4.3, 4.4); Matrix4x4 second = new Matrix4x4(1.1f, 1.2f, 1.3f, 1.4f, 2.1f, 2.2f, 2.3f, 2.4f, 3.1f, 3.2f, 3.3f, 3.4f, 4.1f, 4.2f, 4.3f, 4.4f); var firstDeterminante = first.Determinante; var secondDeterminante = (double)second.GetDeterminant(); Assert.Equal(firstDeterminante, secondDeterminante, ConstantsMath.Precision); }
public void InverseTest_WhenStandardCoordinateSystemIsGiven_ThenInverseMultiplyWithCoordinateSystemIsIdentity() { var vec = Matrix44D.CreateCoordinateSystem(new Position3D(0.0, 0.0, 0.0), new Vector3D(1.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 1.0)); var inverse = vec.Inverse(); var ident = inverse * vec; Assert.Equal(Matrix44D.Identity, ident); }
/// <summary> /// Creates a <see cref="PoseD"/> from a matrix that contains a translation and a rotation. /// </summary> /// <param name="poseMatrix">The pose matrix.</param> /// <returns>A pose that represents the same transformation as the 4x4-matrix.</returns> /// <remarks> /// <paramref name="poseMatrix"/> must only contain rotations and translations, otherwise the /// result is undefined. /// </remarks> public static PoseD FromMatrix(Matrix44D poseMatrix) { Debug.Assert(IsValid(poseMatrix), "Matrix is not a valid pose matrix. Pose matrix must only contain rotations and translations."); return(new PoseD( new Vector3D(poseMatrix.M03, poseMatrix.M13, poseMatrix.M23), new Matrix33D(poseMatrix.M00, poseMatrix.M01, poseMatrix.M02, poseMatrix.M10, poseMatrix.M11, poseMatrix.M12, poseMatrix.M20, poseMatrix.M21, poseMatrix.M22))); }
public static void MoveTargetTo(this Camera camera, Position3D target) { var offset = target - (camera.Target - camera.Frame.Offset); var ex = camera.Frame.Ex; var ey = camera.Frame.Ey; var ez = camera.Frame.Ez; camera.Target = target; camera.Frame = Matrix44D.CreateCoordinateSystem(offset, ex, ey, ez); }
private static Dictionary <Triangle, TriangleHL> ConvertFace(Face face, Matrix44D cameraBodyFrame, double nearPlane) { var faceHL = new FaceHL() { HasBorder = face.HasBorder, HasFacets = face.HasFacets }; var triangleDict = face.Triangles.ToDictionary(t => t, t => ConvertTriangle(t, faceHL, cameraBodyFrame, nearPlane)); return(triangleDict); }
public void CreateRotationTest_WhenParameterOnlyAxisAndAngle_ThenPositioIsOnlyRotated() { var axis = new Vector3D(1, 0, 0); var rot = Matrix44D.CreateRotation(axis, 90.0.DegToRad()); Assert.Equal(new Position3D(), rot.Offset); Assert.Equal(new Vector3D(), rot.Translation); var pos = rot * new Position3D(0.0, 0.0, 1.0); Assert.Equal(new Position3D(0.0, -1.0, 0.0), pos); }
public static Matrix44D ToMatrix44D(this CardanFrame eulerFrame) { var alphaRotation = Matrix44D.CreateRotation(new Vector3D(1.0, 0.0, 0.0), eulerFrame.AlphaAngleAxisX); var betaRotation = Matrix44D.CreateRotation(new Vector3D(0.0, 1.0, 0.0), eulerFrame.BetaAngleAxisY); var gammaRotation = Matrix44D.CreateRotation(new Vector3D(0.0, 0.0, 1.0), eulerFrame.GammaAngleAxisZ); var translation = Matrix44D.CreateTranslation(eulerFrame.Translation); var matrix = translation * gammaRotation * betaRotation * alphaRotation; return(matrix); }
public Matrix44D CreateFrame() { var rotAlpha = Matrix44D.CreateRotation(new Vector3D(1, 0, 0), CreateAngle()); var rotBeta = Matrix44D.CreateRotation(new Vector3D(0, 1, 0), CreateAngle()); var rotGamma = Matrix44D.CreateRotation(new Vector3D(0, 0, 1), CreateAngle()); var translation = Matrix44D.CreateTranslation(CreateVector()); var randomMatrix = translation * rotAlpha * rotBeta * rotGamma; return(randomMatrix); }
private static void Process(LinearSensor linearSensor, Body body, Axis3D startMoveRay, Axis3D endMoveRay) { if (startMoveRay.Offset == endMoveRay.Offset) { return; } var moveVector = CalculateMove(body, linearSensor.Axis, startMoveRay, endMoveRay); var moveFrame = Matrix44D.CreateTranslation(moveVector); body.Frame = moveFrame * body.Frame; }
public void OperatorMultiplyTest_WhenTranslationWithRoationAreMultiplidWithPosition_ThenPositionIsTranslatedAndThenRotated() { var first = Matrix44D.CreateRotation(new Position3D(), new Vector3D(0, 0, 1), 90.0.DegToRad()); var second = Matrix44D.CreateTranslation(new Vector3D(100.0, 0.0, 0.0)); var position = new Position3D(1.0, 0.0, 0.0); var newPosition = first * second * position; var expected = new Position3D(0.0, 101.0, 0.0); Assert.Equal(expected, newPosition); }
public static Scene CreateAndPopulateScene() { var scene = new Scene(); var cube = Cube.Create(10.0); cube.Frame = Matrix44D.CreateTranslation(new Vector3D(0, 0, 5)); scene.Bodies.Add(cube); var cube2 = Cube.Create(10.0); cube2.Frame = Matrix44D.CreateTranslation(new Vector3D(0, 0, 15)) * Matrix44D.CreateRotation(new Vector3D(0, 0, 1), 45.0.DegToRad());; cube2.Sensor = new CylinderSensor(new Vector3D(0, 0, 1)); scene.Bodies.Add(cube2); var cylinder = Cylinder.Create(16, 4.0, 10.0); cylinder.Frame = Matrix44D.CreateTranslation(new Vector3D(10, 10, 5.1)); cylinder.Sensor = new LinearSensor(new Vector3D(0, 1, 0)); scene.Bodies.Add(cylinder); var floor = Floor.Create(4, 20); scene.Bodies.Add(floor); double[][] segments = new double[][] { new [] { 0.0, 0.0, 5.0, 0.0 }, new [] { 5.0, 0.0, 5.0, 1.0 }, new [] { 5.0, 1.0, 1.0, 5.0 }, new [] { 1.0, 5.0, 1.0, 7.0 }, new [] { 1.0, 7.0, 5.0, 11.0 }, new [] { 5.0, 11.0, 5.0, 12.0 }, new [] { 5.0, 12.0, 0.0, 12.0 }, }; bool[] borderFlags = new bool[] { true, true, true, true, true, true, true }; bool[] facetsFlags = new bool[] { false, false, false, false, false, false, false }; var rotationBody = RotationBody.Create(16, segments, borderFlags, facetsFlags); rotationBody.Sensor = new PlaneSensor(new Vector3D(0, 0, 1)); rotationBody.Frame = Matrix44D.CreateTranslation(new Vector3D(30, -30, 0.1)); scene.Bodies.Add(rotationBody); var sphere = Sphere.Create(16, 8); sphere.Sensor = new SphereSensor(); sphere.Frame = Matrix44D.CreateTranslation(new Vector3D(-30, 30, 10)); scene.Bodies.Add(sphere); return(scene); }
public void MultiplicationOperatorTest_WhenMatrixMultpliedWithYAxis_ThenResultVectorIsEyOfMatrix() { var ex = new Vector3D(1.0, 1.0, 0.0); var ey = new Vector3D(-1.0, 1.0, 0.0); var ez = new Vector3D(0.0, 0.0, 1.0); var mat = Matrix44D.CreateCoordinateSystem(new Position3D(), ex, ey, ez); var yAxis = new Vector3D(0.0, 1.0, 0.0); var eyNew = mat * yAxis; Assert.Equal(ey, eyNew); }
public void RandomMatrix44D() { Matrix44D matrix = RandomHelper.Random.NextMatrix44D(-2.0, 0.5); for (int i = 0; i < 16; i++) { Assert.IsTrue(-2.0 <= matrix[i] && matrix[i] <= 0.5); } // Must not throw exception. RandomHelper.NextMatrix44D(null, 1, 3); }
public static void SetCamera(this Camera camera, Position3D target, double alpha, double beta, double distance) { alpha = alpha.DegToRad(); beta = beta.DegToRad(); var rotAlpha = Matrix44D.CreateRotation(new Position3D(), new Vector3D(0, 0, 1), alpha); var rotBeta = Matrix44D.CreateRotation(new Position3D(), new Vector3D(1, 0, 0), -beta); var translation = Matrix44D.CreateTranslation(new Vector3D(0.0, -distance, 0.0)); var targetTranslation = Matrix44D.CreateTranslation(camera.Target.ToVector3D()); var frame = targetTranslation * rotAlpha * rotBeta * translation; camera.Frame = frame; camera.Target = target; }
public static void Zoom(this Camera camera, double delta) { if (delta >= camera.Distance) { delta = camera.Distance; } var offset = camera.Frame.Offset; var ex = camera.Frame.Ex; var ey = camera.Frame.Ey; var ez = camera.Frame.Ez; offset = offset + ey * delta; camera.Frame = Matrix44D.CreateCoordinateSystem(offset, ex, ey, ez); }
public void CreatePlaneCoordinateSystemTest_WhenCoordinateSystemIsCalledWithOffsetAndNormal_ThenTranslationIsOffsetAndEzIsNormal() { var normal = new Vector3D(1.0, 1.0, 1.0).Normalize(); var offset = new Position3D(2, 3, 4); var mat = Matrix44D.CreatePlaneCoordinateSystem(offset, normal); Assert.Equal(offset, mat.Offset); Assert.Equal(offset.ToVector3D(), mat.Translation); Assert.Equal(normal, mat.Ez); Assert.Equal(0.0, mat.Ex * mat.Ey, ConstantsMath.Precision); Assert.Equal(0.0, mat.Ey * mat.Ez, ConstantsMath.Precision); Assert.Equal(0.0, mat.Ez * mat.Ex, ConstantsMath.Precision); }
public void CreateCoordinateSystemTest_WhenCoordinateSystemIsCalledWithExEyEzAndTranslation_ThenExEyEzAndTranslationAreEqualToParameters() { var ex = new Vector3D(1.0, 0.0, 0.0); var ey = new Vector3D(0.0, 0.0, -1.0); var ez = new Vector3D(-1.0, 1.0, 0.0); var offset = new Position3D(2, 3, 4); var mat = Matrix44D.CreateCoordinateSystem(offset, ex, ey, ez); Assert.Equal(ex, mat.Ex); Assert.Equal(ey, mat.Ey); Assert.Equal(ez, mat.Ez); Assert.Equal(offset, mat.Offset); Assert.Equal(offset.ToVector3D(), mat.Translation); }
public void CreateCoordinateSystemTest_WhenCoordinateSystemIsCalledExEz_ThenExEyEzAreEqualToParametersAndAndTranslationIsOrigin() { var ex = new Vector3D(1.0, 0.0, 0.0); var ez = new Vector3D(-1.0, 1.0, 0.0); var mat = Matrix44D.CreateCoordinateSystem(ex, ez); var ey = new Vector3D(0.0, 0.0, -1.0); Assert.Equal(ex, mat.Ex); Assert.Equal(ey, mat.Ey); Assert.Equal(ez, mat.Ez); Assert.Equal(new Position3D(), mat.Offset); Assert.Equal(new Vector3D(), mat.Translation); }
private static void Process( Body body, Position3D bodyTouchPosition, double startX, double startY, Axis3D endMoveRay, Axis3D cylinderAxis, double canvasWidth, double canvasHeight, Camera camera) { var angle = CalculateAngle(bodyTouchPosition, startX, startY, endMoveRay, cylinderAxis, canvasWidth, canvasHeight, camera); var rotation = Matrix44D.CreateRotation(cylinderAxis.Offset, cylinderAxis.Direction, angle); body.Frame = rotation * body.Frame; }
public void Absolute() { Matrix44D absoluteM = new Matrix44D(-1, -2, -3, -4, -5, -6, -7, -8, -9, -10, -11, -12, -13, -14, -15, -16); absoluteM.Absolute(); Assert.AreEqual(1, absoluteM.M00); Assert.AreEqual(2, absoluteM.M01); Assert.AreEqual(3, absoluteM.M02); Assert.AreEqual(4, absoluteM.M03); Assert.AreEqual(5, absoluteM.M10); Assert.AreEqual(6, absoluteM.M11); Assert.AreEqual(7, absoluteM.M12); Assert.AreEqual(8, absoluteM.M13); Assert.AreEqual(9, absoluteM.M20); Assert.AreEqual(10, absoluteM.M21); Assert.AreEqual(11, absoluteM.M22); Assert.AreEqual(12, absoluteM.M23); Assert.AreEqual(13, absoluteM.M30); Assert.AreEqual(14, absoluteM.M31); Assert.AreEqual(15, absoluteM.M32); Assert.AreEqual(16, absoluteM.M33); absoluteM = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); absoluteM.Absolute(); Assert.AreEqual(1, absoluteM.M00); Assert.AreEqual(2, absoluteM.M01); Assert.AreEqual(3, absoluteM.M02); Assert.AreEqual(4, absoluteM.M03); Assert.AreEqual(5, absoluteM.M10); Assert.AreEqual(6, absoluteM.M11); Assert.AreEqual(7, absoluteM.M12); Assert.AreEqual(8, absoluteM.M13); Assert.AreEqual(9, absoluteM.M20); Assert.AreEqual(10, absoluteM.M21); Assert.AreEqual(11, absoluteM.M22); Assert.AreEqual(12, absoluteM.M23); Assert.AreEqual(13, absoluteM.M30); Assert.AreEqual(14, absoluteM.M31); Assert.AreEqual(15, absoluteM.M32); Assert.AreEqual(16, absoluteM.M33); }
public void CreatePlaneCoordinateSystemTest_WhenCoordinateSystemIsCalledWithOffsetAndTwoPlaneVectors_ThenTranslationIsOffsetAndEzIsNormal() { var first = new Vector3D(1.0, 1.0, 0.0).Normalize(); var second = new Vector3D(1.0, -1.0, 0.0).Normalize(); var offset = new Position3D(2, 3, 4); var mat = Matrix44D.CreatePlaneCoordinateSystem(offset, first, second); var expectedNormal = (first & second).Normalize(); Assert.Equal(offset, mat.Offset); Assert.Equal(offset.ToVector3D(), mat.Translation); Assert.Equal(expectedNormal, mat.Ez); Assert.Equal(0.0, mat.Ex * mat.Ey, ConstantsMath.Precision); Assert.Equal(0.0, mat.Ey * mat.Ez, ConstantsMath.Precision); Assert.Equal(0.0, mat.Ez * mat.Ex, ConstantsMath.Precision); }
public static CardanFrame ToCardanFrame(this Matrix44D matrix) { var ex = matrix.Ex; var ey = matrix.Ey; var gammaAngleAxisZ = (ex.X, ex.Y).ToAngle(); var rotationXY = Matrix44D.CreateRotation(new Vector3D(0, 0, 1), -gammaAngleAxisZ); var ex1 = rotationXY * ex; var betaAngleAxisY = -(ex1.X, ex1.Z).ToAngle(); var rotationXZ = Matrix44D.CreateRotation(new Vector3D(0, 1, 0), -betaAngleAxisY); var ey1 = rotationXZ * rotationXY * ey; var alphaAngleAxisX = (ey1.Y, ey1.Z).ToAngle(); return(new CardanFrame(matrix.Translation, alphaAngleAxisX, betaAngleAxisY, gammaAngleAxisZ)); }
private static double CalculateAngleSign(Axis3D cylinderAxis, Position3D start, Position3D end) { var axisFrame = Matrix44D.CreateRotation(cylinderAxis.Offset, cylinderAxis.Direction); Matrix44D invframe = axisFrame.Inverse(); var startInFrame = invframe * start; var endInFrame = invframe * end; var sx = startInFrame.X; var sy = startInFrame.Y; var ex = endInFrame.X; var ey = endInFrame.Y; var sign = TriangleMath.IsCounterClockwise(sx, sy, ex, ey, 0.0, 0.0) ? 1.0 : -1.0; return(sign); }
/// <summary> /// Determines whether the specified matrix is a valid pose matrix. /// </summary> /// <param name="matrix">The matrix.</param> /// <returns> /// <see langword="true"/> if the specified matrix is a valid pose matrix; otherwise, /// <see langword="false"/>. /// </returns> /// <remarks> /// This method makes a simple, low-performance test. /// </remarks> public static bool IsValid(Matrix44D matrix) { Vector4D v1 = matrix * Vector4D.UnitX; Vector4D v2 = matrix * Vector4D.UnitY; Vector4D v3 = matrix * Vector4D.UnitZ; return(Numeric.AreEqual(v1.LengthSquared, 1) && Numeric.AreEqual(v2.LengthSquared, 1) && Numeric.AreEqual(v3.LengthSquared, 1) && Numeric.IsZero(Vector4D.Dot(v1, v2)) && Numeric.IsZero(Vector4D.Dot(v2, v3)) && Numeric.IsZero(Vector4D.Dot(v1, v3)) && Numeric.AreEqual(1.0, matrix.Determinant) && Numeric.IsZero(matrix.M30) && Numeric.IsZero(matrix.M31) && Numeric.IsZero(matrix.M32) && Numeric.AreEqual(matrix.M33, 1)); }
public void ExEyEzTranslationTest_WhenMatrixIsCreated_ThenExEyEzAndTranslationAreColumnsOfMatrix() { var mat = new Matrix44D(1.1, 1.2, 1.3, 1.4, 2.1, 2.2, 2.3, 2.4, 3.1, 3.2, 3.3, 3.4, 0.0, 0.0, 0.0, 1.0); var expectedEx = new Vector3D(1.1, 2.1, 3.1); var expectedEy = new Vector3D(1.2, 2.2, 3.2); var expectedEz = new Vector3D(1.3, 2.3, 3.3); var expectedOffset = new Position3D(1.4, 2.4, 3.4); Assert.Equal(expectedEx, mat.Ex); Assert.Equal(expectedEy, mat.Ey); Assert.Equal(expectedEz, mat.Ez); Assert.Equal(expectedOffset, mat.Offset); Assert.Equal(expectedOffset.ToVector3D(), mat.Translation); }
public void Test1() { PoseD p = PoseD.Identity; Assert.AreEqual(Matrix44D.Identity, p.ToMatrix44D()); Assert.AreEqual(Matrix33D.Identity, p.Orientation); Assert.AreEqual(Vector3D.Zero, p.Position); p.Position = new Vector3D(1, 2, 3); p.Orientation = Matrix33D.CreateRotation(new Vector3D(3, -4, 9), 0.49); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitX), 0), p * new Vector4D(1, 0, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitY), 0), p * new Vector4D(0, 1, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitZ), 0), p * new Vector4D(0, 0, 1, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitX), 1), p * new Vector4D(1, 0, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitY), 1), p * new Vector4D(0, 1, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitZ), 1), p * new Vector4D(0, 0, 1, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitX), 0), p.Inverse * new Vector4D(1, 0, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitY), 0), p.Inverse * new Vector4D(0, 1, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitZ), 0), p.Inverse * new Vector4D(0, 0, 1, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitX), 1), p.Inverse * new Vector4D(1, 0, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitY), 1), p.Inverse * new Vector4D(0, 1, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitZ), 1), p.Inverse * new Vector4D(0, 0, 1, 1))); PoseD p2 = PoseD.FromMatrix(new Matrix44D(p.Orientation, Vector3D.Zero)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation)); Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, Vector3D.Zero)); Matrix44D m = p2; m.SetColumn(3, new Vector4D(p.Position, 1)); p2 = PoseD.FromMatrix(m); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation)); Assert.AreEqual(p.Position, p2.Position); //Assert.IsTrue(Vector3D.AreNumericallyEqual(p.Position, p2.Position)); // Test other constructors. Assert.AreEqual(Vector3D.Zero, new PoseD(QuaternionD.CreateRotationX(0.3)).Position); Assert.AreEqual(Matrix33D.CreateRotationX(0.3), new PoseD(Matrix33D.CreateRotationX(0.3)).Orientation); Assert.AreEqual(new Vector3D(1, 2, 3), new PoseD(new Vector3D(1, 2, 3)).Position); Assert.AreEqual(Matrix33D.Identity, new PoseD(new Vector3D(1, 2, 3)).Orientation); }
public void AffineTest_WhenAffineMatrixIsRead_ThenAffineIsMatrixWithoutTranslation() { var mat = new Matrix44D(1.1, 1.2, 1.3, 1.4, 2.1, 2.2, 2.3, 2.4, 3.1, 3.2, 3.3, 3.4, 4.1, 4.2, 4.3, 4.4); var affine = mat.Affine; Assert.Equal(mat.Ex, affine.Ex); Assert.Equal(mat.Ey, affine.Ey); Assert.Equal(mat.Ez, affine.Ez); Assert.Equal(new Position3D(0.0, 0.0, 0.0), affine.Offset); Assert.Equal(new Vector3D(0.0, 0.0, 0.0), affine.Translation); Assert.Equal(0.0, affine.A41, ConstantsMath.Precision); Assert.Equal(0.0, affine.A42, ConstantsMath.Precision); Assert.Equal(0.0, affine.A43, ConstantsMath.Precision); Assert.Equal(1.0, affine.A44, ConstantsMath.Precision); }
public void Determinant() { Matrix44D m = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); Assert.AreEqual(0, m.Determinant); m = new Matrix44D(1, 2, 3, 4, -3, 4, 5, 6, 2, -5, 7, 4, 10, 2, -3, 9); Assert.AreEqual(1142, m.Determinant); }
public void SerializationXml() { Matrix44D m1 = new Matrix44D(12, 23, 45, 56, 67, 89, 90, 12, 43, 65, 87, 43, 34, -12, 84, 44.3); Matrix44D m2; string fileName = "SerializationMatrix44D.xml"; if (File.Exists(fileName)) File.Delete(fileName); XmlSerializer serializer = new XmlSerializer(typeof(Matrix44D)); StreamWriter writer = new StreamWriter(fileName); serializer.Serialize(writer, m1); writer.Close(); serializer = new XmlSerializer(typeof(Matrix44D)); FileStream fileStream = new FileStream(fileName, FileMode.Open); m2 = (Matrix44D)serializer.Deserialize(fileStream); Assert.AreEqual(m1, m2); }
public void SetColumnException1() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m.SetColumn(-1, Vector4D.One); }
public void SetRowException2() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m.SetRow(4, Vector4D.One); }
public void AreEqualWithEpsilon() { double epsilon = 0.001; Matrix44D m0 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D m1 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m1 += new Matrix44D(0.002); Matrix44D m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m2 += new Matrix44D(0.0001); Assert.IsTrue(Matrix44D.AreNumericallyEqual(m0, m0, epsilon)); Assert.IsFalse(Matrix44D.AreNumericallyEqual(m0, m1, epsilon)); Assert.IsTrue(Matrix44D.AreNumericallyEqual(m0, m2, epsilon)); }
public void SubtractionOperator() { Matrix44D m1 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor) * 3; Matrix44D result = m1 - m2; for (int i = 0; i < 16; i++) Assert.AreEqual(-rowMajor[i] * 2, result[i]); }
public void TestEquals() { Matrix44D m1 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Assert.IsTrue(m1.Equals(m1)); Assert.IsTrue(m1.Equals(m2)); for (int i = 0; i < 16; i++) { m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m2[i] += 0.1; Assert.IsFalse(m1.Equals(m2)); } Assert.IsFalse(m1.Equals(m1.ToString())); }
public void TransformNormal() { // Random matrix Matrix44D transform = new Matrix44D(1, 2, 3, 4, 2, 5, 8, 3, 7, 6, -1, 1, 0, 0, 0, 1); Vector3D p3 = new Vector3D(1.0, 2.0, 0.5); Vector3D x3 = new Vector3D(-3.4, 5.5, -0.5); Vector3D d = (x3 - p3); Vector3D n3 = d.Orthonormal1; Vector4D p4 = new Vector4D(p3.X, p3.Y, p3.Z, 1.0); Vector4D x4 = new Vector4D(x3.X, x3.Y, x3.Z, 1.0); Vector4D n4 = new Vector4D(n3.X, n3.Y, n3.Z, 0.0); double planeEquation = Vector4D.Dot((x4 - p4), n4); Assert.IsTrue(Numeric.IsZero(planeEquation)); p4 = transform * p4; x4 = transform * x4; n3 = transform.TransformNormal(n3); n4 = new Vector4D(n3.X, n3.Y, n3.Z, 0.0); planeEquation = Vector4D.Dot((x4 - p4), n4); Assert.IsTrue(Numeric.IsZero(planeEquation)); }
public void Transpose() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m.Transpose(); Matrix44D mt = new Matrix44D(rowMajor, MatrixOrder.ColumnMajor); Assert.AreEqual(mt, m); Matrix44D i = Matrix44D.Identity; i.Transpose(); Assert.AreEqual(Matrix44D.Identity, i); }
public void Transposed() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D mt = new Matrix44D(rowMajor, MatrixOrder.ColumnMajor); Assert.AreEqual(mt, m.Transposed); Assert.AreEqual(Matrix44D.Identity, Matrix44D.Identity.Transposed); }
public void ClampToZeroStatic() { Matrix44D m = new Matrix44D(0.0000000000001); Assert.AreEqual(new Matrix44D(), Matrix44D.ClampToZero(m)); Assert.AreEqual(new Matrix44D(0.0000000000001), m); // m unchanged? m = new Matrix44D(0.1); Assert.AreEqual(new Matrix44D(0.1), Matrix44D.ClampToZero(m)); Assert.AreEqual(new Matrix44D(0.1), m); m = new Matrix44D(0.001); Assert.AreEqual(new Matrix44D(), Matrix44D.ClampToZero(m, 0.01)); Assert.AreEqual(new Matrix44D(0.001), m); m = new Matrix44D(0.1); Assert.AreEqual(new Matrix44D(0.1), Matrix44D.ClampToZero(m, 0.01)); Assert.AreEqual(new Matrix44D(0.1), m); }
public void ExplicitToXnaCast() { Matrix44D v = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); Matrix xna = (Matrix)v; Assert.AreEqual(xna.M11, v.M00); Assert.AreEqual(xna.M12, v.M10); Assert.AreEqual(xna.M13, v.M20); Assert.AreEqual(xna.M14, v.M30); Assert.AreEqual(xna.M21, v.M01); Assert.AreEqual(xna.M22, v.M11); Assert.AreEqual(xna.M23, v.M21); Assert.AreEqual(xna.M24, v.M31); Assert.AreEqual(xna.M31, v.M02); Assert.AreEqual(xna.M32, v.M12); Assert.AreEqual(xna.M33, v.M22); Assert.AreEqual(xna.M34, v.M32); Assert.AreEqual(xna.M41, v.M03); Assert.AreEqual(xna.M42, v.M13); Assert.AreEqual(xna.M43, v.M23); Assert.AreEqual(xna.M44, v.M33); }
public void EqualityOperators() { Matrix44D m1 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Assert.IsTrue(m1 == m2); Assert.IsFalse(m1 != m2); for (int i = 0; i < 16; i++) { m2 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m2[i] += 0.1; Assert.IsFalse(m1 == m2); Assert.IsTrue(m1 != m2); } }
public void DivisionOperator() { double s = 0.1234; Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m = m / s; for (int i = 0; i < 16; i++) Assert.IsTrue(Numeric.AreEqual(rowMajor[i] / s, m[i])); }
public void TestToString() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Assert.IsFalse(String.IsNullOrEmpty(m.ToString())); }
public void Trace() { Matrix44D m = new Matrix44D(new double[4, 4] { { 1, 2, 3, 4 }, { 5, 6, 7, 8 }, { 9, 10, 11, 12 }, { 13, 14, 15, 16 } }); Assert.AreEqual(34, m.Trace); }
public void ClampToZero() { Matrix44D m = new Matrix44D(0.0000000000001); m.ClampToZero(); Assert.AreEqual(new Matrix44D(), m); m = new Matrix44D(0.1); m.ClampToZero(); Assert.AreEqual(new Matrix44D(0.1), m); m = new Matrix44D(0.001); m.ClampToZero(0.01); Assert.AreEqual(new Matrix44D(), m); m = new Matrix44D(0.1); m.ClampToZero(0.01); Assert.AreEqual(new Matrix44D(0.1), m); }
public void IsValid() { Matrix44D inValidPose = new Matrix44D(new double[,] { {1, 2, 3, 0}, {4, 5, 6, 0}, {7, 8, 9, 0}, {0, 0, 0, 1}, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); Assert.IsTrue(PoseD.IsValid(Matrix44D.CreateRotationZ(0.3))); inValidPose = new Matrix44D(new double[,] { {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 1, 0, 1}, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); inValidPose = new Matrix44D(new double[,] { {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, -1, 0}, {0, 1, 0, 1}, }); Assert.IsFalse(PoseD.IsValid(inValidPose)); }
public void ToMatrixD() { Matrix44D m44 = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); MatrixD m = m44.ToMatrixD(); for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) Assert.AreEqual(i * 4 + j + 1, m[i, j]); m = m44; for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) Assert.AreEqual(i * 4 + j + 1, m[i, j]); }
public void Subtraction() { Matrix44D m1 = new Matrix44D(rowMajor, MatrixOrder.RowMajor); Matrix44D m2 = Matrix44D.One; Matrix44D result = Matrix44D.Subtract(m1, m2); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i] - 1.0, result[i]); }
public void ToMatrix44F() { double m00 = 23.5; double m01 = 0.0; double m02 = -11.0; double m03 = 0.3; double m10 = 33.5; double m11 = 1.1; double m12 = -12.0; double m13 = 0.4; double m20 = 43.5; double m21 = 2.2; double m22 = -13.0; double m23 = 0.5; double m30 = 53.5; double m31 = 3.3; double m32 = -14.0; double m33 = 0.6; Matrix44F matrix44F = new Matrix44D(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33).ToMatrix44F(); Assert.IsTrue(Numeric.AreEqual((float)m00, matrix44F[0, 0])); Assert.IsTrue(Numeric.AreEqual((float)m01, matrix44F[0, 1])); Assert.IsTrue(Numeric.AreEqual((float)m02, matrix44F[0, 2])); Assert.IsTrue(Numeric.AreEqual((float)m03, matrix44F[0, 3])); Assert.IsTrue(Numeric.AreEqual((float)m10, matrix44F[1, 0])); Assert.IsTrue(Numeric.AreEqual((float)m11, matrix44F[1, 1])); Assert.IsTrue(Numeric.AreEqual((float)m12, matrix44F[1, 2])); Assert.IsTrue(Numeric.AreEqual((float)m13, matrix44F[1, 3])); Assert.IsTrue(Numeric.AreEqual((float)m20, matrix44F[2, 0])); Assert.IsTrue(Numeric.AreEqual((float)m21, matrix44F[2, 1])); Assert.IsTrue(Numeric.AreEqual((float)m22, matrix44F[2, 2])); Assert.IsTrue(Numeric.AreEqual((float)m23, matrix44F[2, 3])); Assert.IsTrue(Numeric.AreEqual((float)m30, matrix44F[3, 0])); Assert.IsTrue(Numeric.AreEqual((float)m31, matrix44F[3, 1])); Assert.IsTrue(Numeric.AreEqual((float)m32, matrix44F[3, 2])); Assert.IsTrue(Numeric.AreEqual((float)m33, matrix44F[3, 3])); }
//-------------------------------------------------------------- /// <summary> /// Computes the derived values, like sun/moon positions, transformation matrices and light /// intensities. This method must be called when the location or time has changed. /// </summary> /// <remarks> /// This method must be called when the input properties <see cref="Latitude"/>, /// <see cref="Longitude"/>, <see cref="Altitude"/>, or <see cref="Time"/>) have changed. /// </remarks> public void Update() { _epoch2000Centuries = ToEpoch2000Centuries(Time, true); _epoch1990Days = ToEpoch1990Days(Time, false); // To transform from ecliptic to equatorial, we rotate by the obliquity of the ecliptic. _e = 0.409093 - 0.000227 * _epoch2000Centuries; EclipticToEquatorial = Matrix33D.CreateRotationX(_e); // GMST = Greenwich mean sidereal time (mittlere Greenwich-Sternzeit) in radians. double gmst = 4.894961 + 230121.675315 * ToEpoch2000Centuries(Time, false); EquatorialToGeographic = Matrix33D.CreateRotationZ(-gmst); // The earth axis slowly changes over time (precession). The precession movement repeats // itself approx. all 26000 years. When we move from to horizontal or geographics, // we need to apply the precession. // In Game Engine Gems: //var Rx = Matrix33D.CreateRotationX(0.1118 * _epoch2000Centuries); //var Ry = Matrix33D.CreateRotationY(-0.00972 * _epoch2000Centuries); //var Rz = Matrix33D.CreateRotationZ(0.01118 * _epoch2000Centuries); //var precession = Rz * (Ry * Rx); // In original article: var Ry = Matrix33D.CreateRotationY(-0.00972 * _epoch2000Centuries); var Rz = Matrix33D.CreateRotationZ(0.01118 * _epoch2000Centuries); var precession = Rz * Ry * Rz; // In game engine gems precession is applied in EclipticToWorld and in // EquatorialToWorld. This makes no sense since precession cannot be valid for both // coordinate systems. --> We assume the precession is given in equatorial space. //EclipticToWorld = rLat * rLong * EclipticToEquatorial * precession; // Latitude rotation var rLat = Matrix33D.CreateRotationY(MathHelper.ToRadians(Latitude) - ConstantsD.PiOver2); // Longitude rotation // LMST = Local mean sidereal time (mittlere Ortssternzeit) in radians. double lmst = gmst + MathHelper.ToRadians(Longitude); var rLong = Matrix33D.CreateRotationZ(-lmst); // Earth radius at the equator. (We assume a perfect sphere. We do not support geodetic // systems with imperfect earth spheres.) const double earthRadius = 6378.137 * 1000; var equatorialToHorizontalTranslation = new Vector3D(0, -earthRadius - Altitude, 0); // Switching of the coordinate axes between Equatorial (z up) and Horizontal (y up). var axisSwitch = new Matrix33D(0, 1, 0, 0, 0, 1, 1, 0, 0); EquatorialToWorld = new Matrix44D(axisSwitch * rLat * rLong * precession, equatorialToHorizontalTranslation); _equatorialToWorldNoPrecession = new Matrix44D(axisSwitch * rLat * rLong, equatorialToHorizontalTranslation); //WorldToGeographic = EquatorialToGeographic * EquatorialToWorld.Minor.Transposed; ComputeSunPosition(); ComputeMoonPosition(); ComputeEarthPosition(); //for (int i = 0; i < NumberOfPlanets; i++) //{ // var planet = (VisiblePlanets)i; // if (planet != VisiblePlanets.Earth) // ComputePlanetData(planet); //} }
public void ToList() { Matrix44D m = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); IList<double> list = m.ToList(MatrixOrder.RowMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], list[i]); list = m.ToList(MatrixOrder.ColumnMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(columnMajor[i], list[i]); }
public void SetRow() { Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); m.SetRow(0, new Vector4D(0.1, 0.2, 0.3, 0.4)); Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetRow(0)); Assert.AreEqual(new Vector4D(5.0, 6.0, 7.0, 8.0), m.GetRow(1)); Assert.AreEqual(new Vector4D(9.0, 10.0, 11.0, 12.0), m.GetRow(2)); Assert.AreEqual(new Vector4D(13.0, 14.0, 15.0, 16.0), m.GetRow(3)); m.SetRow(1, new Vector4D(0.4, 0.5, 0.6, 0.7)); Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetRow(0)); Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetRow(1)); Assert.AreEqual(new Vector4D(9.0, 10.0, 11.0, 12.0), m.GetRow(2)); Assert.AreEqual(new Vector4D(13.0, 14.0, 15.0, 16.0), m.GetRow(3)); m.SetRow(2, new Vector4D(0.7, 0.8, 0.9, 1.0)); Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetRow(0)); Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetRow(1)); Assert.AreEqual(new Vector4D(0.7, 0.8, 0.9, 1.0), m.GetRow(2)); Assert.AreEqual(new Vector4D(13.0, 14.0, 15.0, 16.0), m.GetRow(3)); m.SetRow(3, new Vector4D(1.7, 1.8, 1.9, 1.3)); Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetRow(0)); Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetRow(1)); Assert.AreEqual(new Vector4D(0.7, 0.8, 0.9, 1.0), m.GetRow(2)); Assert.AreEqual(new Vector4D(1.7, 1.8, 1.9, 1.3), m.GetRow(3)); }
public void ToArrayJagged() { Matrix44D m = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); double[][] array = m.ToArrayJagged(); for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) Assert.AreEqual(i * 4 + j + 1, array[i][j]); array = (double[][])m; for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) Assert.AreEqual(i * 4 + j + 1, array[i][j]); }
public void SerializationXml2() { Matrix44D m1 = new Matrix44D(12, 23, 45, 56, 67, 89, 90, 12, 43, 65, 87, 43, 34, -12, 84, 44.3); Matrix44D m2; string fileName = "SerializationMatrix44D_DataContractSerializer.xml"; if (File.Exists(fileName)) File.Delete(fileName); var serializer = new DataContractSerializer(typeof(Matrix44D)); using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write)) using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8)) serializer.WriteObject(writer, m1); using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read)) using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas())) m2 = (Matrix44D)serializer.ReadObject(reader); Assert.AreEqual(m1, m2); }
public void ToArray1D() { Matrix44D m = new Matrix44D(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16); double[] array = m.ToArray1D(MatrixOrder.RowMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], array[i]); array = m.ToArray1D(MatrixOrder.ColumnMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(columnMajor[i], array[i]); }
public void DecomposeShouldFail() { Matrix44D matrix = new Matrix44D(); Vector3D scaleOfMatrix; QuaternionD rotationOfMatrix; Vector3D translationOfMatrix; bool result = matrix.Decompose(out scaleOfMatrix, out rotationOfMatrix, out translationOfMatrix); Assert.IsFalse(result); matrix = new Matrix44D(rowMajor, MatrixOrder.RowMajor); result = matrix.Decompose(out scaleOfMatrix, out rotationOfMatrix, out translationOfMatrix); Assert.IsFalse(result); }
public void Constructors() { Matrix44D m = new Matrix44D(1.0, 2.0, 3.0, 4.0f, 5.0, 6.0, 7.0, 8.0f, 9.0, 10.0, 11.0, 12.0f, 13.0, 14.0, 15.0, 16.0); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(columnMajor, MatrixOrder.ColumnMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(rowMajor, MatrixOrder.RowMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(new List<double>(columnMajor), MatrixOrder.ColumnMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(new List<double>(rowMajor), MatrixOrder.RowMajor); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(new double[4, 4] { { 1, 2, 3, 4 }, { 5, 6, 7, 8 }, { 9, 10, 11, 12 }, { 13, 14, 15, 16}}); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(new double[4][] { new double[4] { 1, 2, 3, 4 }, new double[4] { 5, 6, 7, 8 }, new double[4] { 9, 10, 11, 12 }, new double[4] { 13, 14, 15, 16}}); for (int i = 0; i < 16; i++) Assert.AreEqual(rowMajor[i], m[i]); m = new Matrix44D(new Matrix33D(1, 2, 3, 4, 5, 6, 7, 8, 9), new Vector3D(10, 11, 12)); Assert.AreEqual(new Matrix44D(1, 2, 3, 10, 4, 5, 6, 11, 7, 8, 9, 12, 0, 0, 0, 1), m); }