C# SimilarityFactorMMS

        //-----------------------------------------------------------------------------------------------------------------------------
        private int SimilarityFactor(Color A, Color B)
        {
            return (int)Math.Sqrt(Math.Pow((A.R - B.R), 2) + Math.Pow((A.G - B.G), 2) + Math.Pow((A.B - B.B), 2));
        }
        public Bitmap ColorSimilarityFilter(Bitmap bmp, Color color, Point startPosition, int similarityFactor)
        {
            Bitmap newBmp = new Bitmap(bmp);
            HashSet<Point> pointsToCompare = new HashSet<Point>(); // O(1)
            HashSet<Point> pointsDone = new HashSet<Point>();
            pointsToCompare.Add(startPosition); //first point to compare

            Color Y = bmp.GetPixel(startPosition.X, startPosition.Y);
            Color X = color;

            while (pointsToCompare.Count != 0)
            {
                //
                Point T = pointsToCompare.First();

                //up
                if (T.Y - 1 > 0  && !pointsDone.Contains(new Point(T.X, T.Y - 1)))
                {
                    if (SimilarityFactor(Y, bmp.GetPixel(T.X, T.Y - 1)) <= similarityFactor)
                        pointsToCompare.Add(new Point(T.X, T.Y - 1));
                }

                //down
                if (T.Y + 1 < bmp.Height && !pointsDone.Contains(new Point(T.X, T.Y + 1)))
                {
                    if (SimilarityFactor(Y, bmp.GetPixel(T.X, T.Y + 1)) <= similarityFactor)
                        pointsToCompare.Add(new Point(T.X, T.Y + 1));
                }

                //left
                if (T.X - 1 > 0 && !pointsDone.Contains(new Point(T.X - 1, T.Y)))
                {
                    if (SimilarityFactor(Y, bmp.GetPixel(T.X - 1, T.Y)) <= similarityFactor)
                        pointsToCompare.Add(new Point(T.X - 1, T.Y));
                }

                //right
                if (T.X + 1 < bmp.Width && !pointsDone.Contains(new Point(T.X + 1, T.Y)))
                {
                    if (SimilarityFactor(Y, bmp.GetPixel(T.X + 1, T.Y)) <= similarityFactor)
                        pointsToCompare.Add(new Point(T.X + 1, T.Y));
                }


                newBmp.SetPixel(T.X, T.Y, X);
                pointsDone.Add(T);
                pointsToCompare.Remove(T);

            }

            return newBmp;

        }
        //-----------------------------------------------------------------------------------------------------------------------------