Vector3.TransformPoint bug

MichalZatopek

Hi,

I think I have found a mathematical bug in SDK (ABB.Robotics.Math). When I am trying to use method Vector3.TransformPoint, I am getting diffrent results than I have expected.

From my point of view it should be working like this (pseudo equation - I hope it is understandable):

from - original coordinate system
to - new coordinate system
point  - original point
result - result point in new coordinate system

from * point = to * result 
to.Inverse() * from * point = to.Inverse() * to * result
to.Inverse() * from * point = result 

So I have made my own method, it looks like this:

public static Vector3 TransformPointCorrect(this Vector3 point, Matrix4 fromMatrix4, Matrix4 toMatrix4)

{

      return (toMatrix4.Inverse() * fromMatrix4).MultiplyPoint(point);

}

Please could you check out, if there is a mistake? Or do I understand this method wrong?
It looks like you have changed the order of matrices (the order is important in matrix math).

Thank you!

John_Verheij

It is the otherway around. Most of the time it is easier to follow the arrows. 

"point" is defined in "from". To define it in "to", you have to follow the arrows. So thus first the inverse of "from" and then following "to". So your function will be:

<div>public static Vector3 TransformPointCorrect(this Vector3 point, Matrix4 fromMatrix4, Matrix4 toMatrix4)
</div><div>{
</div><div>&nbsp; &nbsp; &nbsp; return (fromMatrix4.Inverse() * toMatrix4).MultiplyPoint(point);
</div><div>}
</div>

MichalZatopek

Hi John, 

Thanks for your answer. But I cannot agree with you. I have tried a simple demo with your method:

 private void Calculate()
        {
            var A = Matrix4.Identity;
            var B = Matrix4.Identity;
            B.Rotate(Vector3.ZVector, Math.PI /2);

            var x = Vector3.XVector;
            var y = x.TransformPointCorrect(A, B);

            var cond = A.MultiplyPoint(x).AlmostEquals(B.MultiplyPoint(y));
        }

 The coordinate system B is just rotated about 90° in positive direction around Z-axis. The y result should be [0,-1,0]. In your case it is [0,1,0]. I drew it on the paper and I still think that my calculation is correct.

John_Verheij

You are right, my function is indeed incorrect But thinking about your initial question, in which case is in your opinion the "Vector3.TransformPoint" incorrect?

MichalZatopek

I think in SDK it is like this (swapped fromMatrix4 and toMatrix4):

public static Vector3 TransformPointCorrect(this Vector3 point, Matrix4 fromMatrix4, Matrix4 toMatrix4)
{      return (fromMatrix4 * toMatrix4.Inverse()).MultiplyPoint(point);}

 It means that you will get different result than in my case (in matrix math the order of multiplying is important). And I think that my version is correct.

John_Verheij

I agree with you. The calculation is incorrect when both matrices are not the identity matrix. As simple example the following case can be used:



Visually it is already clear that "pA" has the value [1,0,0] in frame "A" and the value [2,-1,0] in frame "B".



The values "pB1" and "pB2" should have the same value...

Maybe one of the admins should have a look at it and create a bug-rapport...