Use glm instead of custom math code.