well like promised heres the code for this algorithm.(if anyone wants it).
its pretty clean i think, not highly optimised, (i use stl vectors, so i suggest if anyone is to use it, convert these to arrays). Probably a few more optimisation could be made.
Also i threw in a sample map, but no doubt anyone who uses it will have their own map format, so it should be easy to change to plugin in theirs.
heres the link, its on the top of the page.
p.s. ill try to post a map builder soon, which automates the creation of these maps.