changed atan to atan2, added matrixmath library