public InterpOperation(ArmController arm, Vector3D worldtarg, double WristPitch, double WristYaw)
{
this.arm = arm;
CurrentState = arm.GetCurrentState();
EndState = arm.FindAnglesState(worldtarg, WristPitch, WristYaw);
EndPos = worldtarg;
CurrentPos = arm.FindPos(out StartWristPitch, out StartWristYaw);
//CurrentPos += arm.wristYaw.WorldMatrix.Up * arm.headHeight;
stepAmount = stepSize / Vector3D.Distance(CurrentPos, EndPos);
// StartWristPitch = arm.wristPitch.Angle + arm.shoulderPitch.Angle - arm.Elbow.Angle;
// StartWristYaw = arm.wristYaw.Angle - arm.shoulderYaw.Angle;
EndWristPitch = WristPitch;
EndWristYaw = WristYaw;
}