math.h & test_math.cpp

math.h altered for FMatrix parametre.
test_math.cpp added for testing math.h.