Add a common function called transformPointWithNormal

This is a simple but common function and has passed the unit test.
Oh, I see. So I have modified the code to use Eigen's matrix multiplication, both in transformPoint and transformPointWithNormal. And the unit test is also passed.