public void PointingUp()
{
double shoulderYaw = -_90Deg;
double shoulderPitch = _90Deg;
Point3 point = ForwardKinematics.GetElbowPointRight(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 expectedPoint = new Point3(
shoulderPosition.X,
shoulderPosition.Y + upperArmLength,
shoulderPosition.Z
);
Assert.AreEqual(expectedPoint, point);
}
public void Origin()
{
double shoulderYaw = 0;
double shoulderPitch = 0;
Point3 point = ForwardKinematics.GetElbowPointRight(shoulderPosition, upperArmLength, shoulderYaw, shoulderPitch);
Point3 expectedPoint = new Point3(
shoulderPosition.X + upperArmLength,
shoulderPosition.Y,
shoulderPosition.Z
);
Assert.AreEqual(expectedPoint, point);
}
