/// <summary>
/// Translates along the current X, Y, and Z axis.
/// </summary>
/// <param name="translation"></param>
/// <returns></returns>
public HomogeneousTransformation TranslateByCurrentOrientation(TranslationVector translation)
{
HomogeneousTransformation transHT = new HomogeneousTransformation(translation);
TranslationVector relTranslation = this.DotProduct(transHT).TranslationVector;
relTranslation = new TranslationVector(relTranslation, TranslationVector.ToFrame, relTranslation.ToFrame);
return(new HomogeneousTransformation(RotationMatrix, TranslationVector + relTranslation, BaseFrame, translation.ToFrame));
}
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);
}
public HomogeneousTransformation DotProduct(HomogeneousTransformation other)
{
FrameTransformationMatrix newVals = base.DotProduct(other);
return(new HomogeneousTransformation(newVals, BaseFrame, other.ToFrame));
}
public HomogeneousTransformation Rotate(RotationMatrix rotation)
{
HomogeneousTransformation rotationHomogeneousTransformation = new HomogeneousTransformation(rotation);
return(new HomogeneousTransformation(DotProduct(rotationHomogeneousTransformation), BaseFrame, ToFrame));
}
