在這邊發問C++的問題，通常得不到什麼正面的答案的。這邊寫程式的人不多哩(或有但都沒跳出來)。

小弟的數學不好，資料結構上關於數學的演算法幾乎都不會，幫不了你了。
不過你可以試著去「C++深度論壇」發問，或者自己試著使用STL函式庫。