Added new function to RTMath and updated readme