public HomogeneousTransformation(RotationMatrix rotationMatrix, TranslationVector translationVector) : this(GenerateHomogenousMatrix(rotationMatrix, translationVector), rotationMatrix.BaseFrame, rotationMatrix.ToFrame) { if (rotationMatrix.BaseFrame != translationVector.BaseFrame && !rotationMatrix.BaseFrame.Equals(translationVector.BaseFrame)) { throw new ArgumentException("base frame and to frame must be the same for rotation matrix and translation vector, they are \"" + rotationMatrix.BaseFrame + "\" and \"" + translationVector.BaseFrame + "\""); } if (rotationMatrix.ToFrame != translationVector.ToFrame && !rotationMatrix.ToFrame.Equals(translationVector.ToFrame)) { throw new ArgumentException("base frame and to frame must be the same for rotation matrix and translation vector, they are \"" + rotationMatrix.ToFrame + "\" and \"" + translationVector.ToFrame + "\""); } }
public HomogeneousTransformation(Matrix values, Frame baseFrame, Frame toFrame) : base(values.Values, baseFrame, toFrame) { if (values.Rows != 4 || values.Cols != 4) { throw new ArgumentException("must be a 4x4 matrix", nameof(values)); } Matrix valuesBottomRow = values.SubMatrix(3, 4, 0, 4).Simplify(); if (!valuesBottomRow.Equals(BottomRowVector)) { throw new ArgumentException($"bottom row must be equal to {BottomRowVector}, but is instead {valuesBottomRow}", nameof(values)); } RotationMatrix = new RotationMatrix(SubMatrix(0, 3, 0, 3), BaseFrame, ToFrame); TranslationVector = new TranslationVector(SubMatrix(0, 3, 3, 4), BaseFrame, ToFrame); }
private static Matrix GenerateHomogenousMatrix(RotationMatrix rotation, TranslationVector translation) { Node[,] values = new Node[4, 4]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { values[i, j] = rotation.Values[i, j]; } } values[0, 3] = translation.X; values[1, 3] = translation.Y; values[2, 3] = translation.Z; values[3, 0] = SillyParser.GetInstance().m(0); values[3, 1] = SillyParser.GetInstance().m(0); values[3, 2] = SillyParser.GetInstance().m(0); values[3, 3] = SillyParser.GetInstance().m(1); return(new Matrix(values)); }
public void TranslateByCurrentOrientationTest() { Frame a = new Frame("a"); Frame b = new Frame("b"); Frame c = new Frame("c"); RotationMatrix rotationMatrix = new RotationMatrix(CartesianDimension.Z, Math.PI / 2, a, b); HomogeneousTransformation originalTransformation = new HomogeneousTransformation(rotationMatrix); TranslationVector translation = new TranslationVector(genNodes(new double[, ] { { 1, 0, 0 } }), b, c); HomogeneousTransformation result = originalTransformation.TranslateByCurrentOrientation(translation); TranslationVector expectedOrigin = new TranslationVector(genNodes(new double[, ] { { 0, 1, 0 } }), a, c).Transform(); RotationMatrix expectedRotation = new RotationMatrix(rotationMatrix, a, c); expectedOrigin.Equals(result.RotationMatrix); result = result.Simplify(); Assert.AreEqual(expectedOrigin, result.TranslationVector); Assert.AreEqual(new HomogeneousTransformation(expectedRotation, expectedOrigin), result); }
private HomogeneousTransformation(RotationMatrix rotationMatrix, TranslationVector translationVector, Frame baseFrame, Frame toFrame) : this(GenerateHomogenousMatrix(rotationMatrix, translationVector), baseFrame, toFrame) { }
public HomogeneousTransformation(RotationMatrix rotationMatrix) : this( GenerateHomogenousMatrix(rotationMatrix, new TranslationVector(genNodes(new double[, ] { { 0 }, { 0 }, { 0 } }))), rotationMatrix.BaseFrame, rotationMatrix.ToFrame) { }
public HomogeneousTransformation Rotate(RotationMatrix rotation) { HomogeneousTransformation rotationHomogeneousTransformation = new HomogeneousTransformation(rotation); return(new HomogeneousTransformation(DotProduct(rotationHomogeneousTransformation), BaseFrame, ToFrame)); }
public RotationMatrix Sum(RotationMatrix other) { Matrix newMatrix = base.Sum(other); return(new RotationMatrix(newMatrix.Values, BaseFrame, ToFrame)); }
public RotationMatrix DotProduct(RotationMatrix other) { Matrix result = base.DotProduct(other); return(new RotationMatrix(result.Values)); }