Added Vector3d normalize