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