I've posted a simple implementation of this some time ago on StackOverflow. It's written in Python, but maybe it can still help you:
► http://stackoverflow.com/a/7294148/627005https://stackoverflow.com/a/7294148/627005
replaced http://gamedev.stackexchange.com/ with https://gamedev.stackexchange.com/