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));
}
