Wrote 2 basic physics functions in C++