BEPUphysics.Constraints.TwoEntity.Motors.TwistMotor.GetDistanceFromGoal C# (CSharp) Метод

        private float GetDistanceFromGoal(float angle)
        {
            float forwardDistance;
            float goalAngle = MathHelper.WrapAngle(settings.servo.goal);
            if (goalAngle > 0)
            {
                if (angle > goalAngle)
                    forwardDistance = angle - goalAngle;
                else if (angle > 0)
                    forwardDistance = MathHelper.TwoPi - goalAngle + angle;
                else //if (angle <= 0)
                    forwardDistance = MathHelper.TwoPi - goalAngle + angle;
            }
            else
            {
                if (angle < goalAngle)
                    forwardDistance = MathHelper.TwoPi - goalAngle + angle;
                else //if (angle < 0)
                    forwardDistance = angle - goalAngle;
                //else //if (currentAngle >= 0)
                //    return angle - myMinimumAngle;
            }
            return forwardDistance > MathHelper.Pi ? MathHelper.TwoPi - forwardDistance : -forwardDistance;
        }
    }