# Vector3.TransformPoint bug

**URL:** <https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777>\
**Category:** Developer Tools\
**Tags:** robotstudio-sdk\
**Created:** [June 20, 2017, 8:47am UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777 "2017-06-20T08:47:57Z")\
**Posts on this page:** 6\
**Page:** 1

<div class="post-metadata">

**Author:** ![MichalZatopek](https://avatars.discourse-cdn.com/v4/letter/m/7ba0ec/32.png) [@MichalZatopek](https://tech-community.robotics.abb.com/u/MichalZatopek)\
**Post date:** [June 20, 2017, 8:47am UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/1 "2017-06-20T08:47:57Z")

</div>

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!

---

<div class="post-metadata">

**Author:** ![John\_Verheij](https://dub1.discourse-cdn.com/flex005/user_avatar/tech-community.robotics.abb.com/john_verheij/32/1724_2.png) [@John\_Verheij](https://tech-community.robotics.abb.com/u/John_Verheij)\
**Post date:** [June 21, 2017, 2:43pm UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/2 "2017-06-21T14:43:30Z")

</div>

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

 ![image.png](https://europe1.discourse-cdn.com/flex005/uploads/robotics1/original/1X/aede411905fce5b2362dbf810533ece17ad9f331.png)  
“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:

```auto

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

{

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

}

```

---

<div class="post-metadata">

**Author:** ![MichalZatopek](https://avatars.discourse-cdn.com/v4/letter/m/7ba0ec/32.png) [@MichalZatopek](https://tech-community.robotics.abb.com/u/MichalZatopek)\
**Post date:** [June 30, 2017, 6:41am UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/3 "2017-06-30T06:41:36Z")

</div>

Hi John,

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

```auto
 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.

 ![image.jpg](https://europe1.discourse-cdn.com/flex005/uploads/robotics1/original/2X/f/f7011150c617d4ebea6df6955fb94a47e9f96424.jpeg)

---

<div class="post-metadata">

**Author:** ![John\_Verheij](https://dub1.discourse-cdn.com/flex005/user_avatar/tech-community.robotics.abb.com/john_verheij/32/1724_2.png) [@John\_Verheij](https://tech-community.robotics.abb.com/u/John_Verheij)\
**Post date:** [July 6, 2017, 9:58am UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/4 "2017-07-06T09:58:32Z")

</div>

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?

---

<div class="post-metadata">

**Author:** ![MichalZatopek](https://avatars.discourse-cdn.com/v4/letter/m/7ba0ec/32.png) [@MichalZatopek](https://tech-community.robotics.abb.com/u/MichalZatopek)\
**Post date:** [July 13, 2017, 11:41am UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/5 "2017-07-13T11:41:57Z")

</div>

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

```auto
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.

---

<div class="post-metadata">

**Author:** ![John\_Verheij](https://dub1.discourse-cdn.com/flex005/user_avatar/tech-community.robotics.abb.com/john_verheij/32/1724_2.png) [@John\_Verheij](https://tech-community.robotics.abb.com/u/John_Verheij)\
**Post date:** [July 24, 2017, 12:19pm UTC](https://tech-community.robotics.abb.com/t/vector3-transformpoint-bug/7777/6 "2017-07-24T12:19:31Z")

</div>

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:

 ![image.png](https://europe1.discourse-cdn.com/flex005/uploads/robotics1/original/2X/c/c30294af1acf2c5335f25f09474b24c230cf1a75.png)

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”.

 ![image.png](https://europe1.discourse-cdn.com/flex005/uploads/robotics1/original/2X/7/7c468a496bb913eca91f0a4059420e88a3a7a135.png)

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…