Hello esteemed colleagues!
I have a CRX5 with iRVision 2D and I am playing around with the math to translate the coordinates of a Point in a UF (found using iRvision) to WF, which I will later create a frame with. As a proof-of-concept, I made a Python program that does (I don't have Karel):
1 - Create the Rotation matrix with given UF Coordinates (by matrix multiplying the rotation matrices of R(X)@R(Y)@R(Z) );
2 - Create the Homogeneous Transformation Matrix by concatenating the Rotation Matrix calculated above, the XYZ coordinates of the UF and [0,0,0,1];
3 - Matrix multiply the Homogeneous Transformation Matrix with the Point XYZ coordinates to get the WF coordinates of that Point;
This part is working great, and is giving me the right results. However it only gives me the XYZ coordinates, so how can I get the angles? I noticed that R is pretty straightforward, just the Point(R) + UF(R) and you get the R angle in WF, but how about W and P?
Thanks in advance for the help!