Make general purpose math header.

[skallaher]

This would entail a creating a header file in the utility directory containing any mathematical operations used repeatedly in systems such as the control system and localization system.

[irwineffect]

Let's also add an Euclidian distance function, for both 2 and 3 dimensions:

double euclidian_distance(double x1, double y1, double x2, double y2);
double euclidian_distance(double x1, double y1, double z1, double x2, double y2, double z2);

I don't think the function definitions should go in the math header itself though. We should make a separate math.cpp file to hold them.

[lucy9tran]

@bkallaher Where is the simulation branch/folder? I think you said there were math functions needed for the math header there too?

[skallaher]

@lucy9tran, the simulator is a separate repository here. Look at the simulator bridge in particular. Don't worry about making the simulator reference your code, we can have @BigDCross do so when you're merged into dev. On another note, you should talk with @irwineffect about having a cpp file for your actual source code and how to modify the CMakeLists file.