I decided to upload my code to help out everyone who is interested in implementing this stuff in thier own projects/games.
My code is based off of this tutorial:
Which is implemented in java.
I took this approach and implemented it into C++ and SDL, with SDL_gfx to draw the primatives etc.
The gfx headers are included in the folder.
So all you need to do is create a new project put all of those files in it, and link SDL.lib and SDL_main.lib/ set up your SDL directories.
You should be good to go at that point.
Should you be unfamiliar with SDL, Here is a great place to help you get the project working:
I hope my code help's!