public void Inline()
{
Vector3 shoulderToElbow = new Vector3(1, 0, 0);
double pitch = InverseKinematics.GetShoulderPitch(shoulderL2R, shoulderToElbow);
Assert.AreEqual(0, pitch, angleTolerance);
}
public void ElbowPointingUp()
{
Vector3 shoulderToElbow = new Vector3(0, 1, 0);
double pitch = InverseKinematics.GetShoulderPitch(shoulderL2R, shoulderToElbow);
Assert.AreEqual(-90, pitch, angleTolerance);
}
public void ElbowPointingForward()
{
Vector3 shoulderToElbow = new Vector3(0, 0, -1);
double pitch = InverseKinematics.GetShoulderPitch(shoulderR2L, shoulderToElbow);
Assert.AreEqual(-90, pitch, angleTolerance);
}
