I can't help you with specific C/C++ source code but if you are interested I can send you a copy of an article (1-page only) taken from "Collected Algorithms of the CACM" (CALGO) which, if you are familiar with C/C++ you should be able to convert into suitable code. The actual reference is CALGO algorithm 199.
If you would like a copy, send me your email address. Mine is "prmyers@acslink.net.au".
I hope this might be of some help.
Paul Myers