クラスなどの配列の中で同じ要素のものを固めて整列させる

最初に結論から

コード

extension Array {

    /// 配列を要素が同じ物が並ぶように整列します。(元の配列を変更します)
    ///
    /// - Parameter by: 並べる基準となる要素の配列
    mutating func align<T: Equatable>(by: [T?]){
        var by = by
        guard self.count == by.count, var now : T? = by.first else {
            return
        }
        for index in 0..<self.count{
            if now != by[index]{
                for jndex in index..<self.count{
                    if now == by[jndex]{
                        let temp = (self[jndex], by[jndex])
                        self.remove(at: jndex)
                        by.remove(at: jndex)
                        self.insert(temp.0, at: index)
                        by.insert(temp.1, at: index)
                        break
                    }
                }
            }
            now = by[index]
        }
    }

    /// 配列を要素が同じ物に並ぶように整列します。(元の配列は変わりません)
    ///
    /// - Parameter by: 並べる基準となる要素の配列
    /// - Returns: 整列した配列
    func aligned<T: Equatable>(by: [T?])->Array{
        var copied = self
        copied.align(by: by)
        return copied
    }

}

表題が何言ってるか分からんって方に

正直自分でもなんて言っていいか分からない。
メンバにString型とかを持つ自作クラスの配列を，そのメンバが同じ物が並ぶように整列させたいとか思ったことないですか。
私はあります。だから作りました。
こんな感じで使います。

class foo{
    var str :String
    init(str: String) {
        self.str = str
    }
}

var bar : [foo] = [foo(str: "a"), foo(str: "b"), foo(str: "c"), foo(str: "b"), foo(str: "a"), foo(str: "b")]

print(bar.aligned(by: bar.map({$0.str})).map({$0.str})) //["a", "a", "b", "b", "b", "c"]

並べ替えのアルゴリズムについて

多分というか絶対，これ以上に最適なアルゴリズムが存在するので，もしアイデアがあればご教授いただければ幸いです。