# How to Form Rigid Body Transformation Matrices

We have two line segments specified by a pair of points $$(b_1,t_1)$$, and $$(b_2,t_2)$$. We want to find the transformation between the two line segments. Define a unit vector $$z_1$$ from $$b_1$$ and pointing towards $$t_1$$. Similarly, define $$z_2$$. Then, I want to find some translation vector and rotation matrix, such that any point in $$z_2$$ can be expressed in terms from $$z_1$$. I understand that the problem is slightly ill-posed because there can be multiple solutions.

Define points and get rotation matrices:

b1 = {1, 0, 1}; t1 = {1, 2, 1};
b2 = {0, 1, 0}; t2 = {1, 1, 0};
z1 = (t1 - b1)/Norm[(t1 - b1)]; z2 = (t2 - b2)/Norm[(t2 - b2)];
Rot1 = RotationMatrix[{{0, 0, 1}, z1}]
Rot2 = RotationMatrix[{{0, 0, 1}, z2}]


Get $$4\times 4$$ transformation matrices:

T1 = Join[MapThread[Append, {Rot1, b1}], {{0, 0, 0, 1}}]
T2 = Join[MapThread[Append, {Rot2, b2}], {{0, 0, 0, 1}}]
T21 = T2 . Inverse[T1]

{r1, l1, r2, l2} = {0.1, Norm[b1 - t1], 0.1, Norm[b2 - t2]};
t21 = (T21 . {0, 0, 0, 1})[[1 ;; 3]];


The distances between the points don't hold. They should remain the same after transformation.

Norm[b2 - b1]
Norm[t21]


Position and orientation of line segments after transformation

Show[Graphics3D[{Cylinder[{{0, 0, 0}, {0, 0, l1}}, r1]}],
Graphics3D[Cylinder[{t21, (T21 . {0, 0, l2, 1})[[1 ;; 3]]}, r2]],
Boxed -> False]


Position and orientation of line segments before transformation

Show[Graphics3D[{Cylinder[{b1, t1}, r1]}],
Graphics3D[Cylinder[{b2, t2}, r1]], Boxed -> False]


The two figures look different from each other. Also, distance between points are not maintained. Am I using RotationMatrix correctly? Also, I might be wrong about mathematical concept of transformation.

Edit(Clarification): Consider a coordinate system {1} such that $$z_1$$ (line segment 1) is aligned with the $$z$$-axis of the coordinate system {1} and origin at $$b_1$$. Similarly, let $$z$$-axis of coordinate system {2} align with $$z_2$$ (line segment 2) and origin at $$b_2$$. Then I want to express the second line segment in terms of the coordinate system {1} in the form of some translation and rotation.

• I'm really not clear on what you're trying to do. Do you want a transformation $T$ such that $T(b_1) = b_2$ and $T(t_1) = t_2$? If so, it won't be an orthogonal transformation, since $|b_2 - t_2| \neq |b_1 - t_1|$. Can you edit your question to include more details about which vectors you want to be mapped to which other vectors? Commented Jun 8, 2021 at 14:02
• Consider a coordinate system {1} such that $z_1$ is aligned with the $z$-axis of the coordinate system {1} and origin at $b_1$. Similarly, let $z$-axis of coordinate system {2} align with $z_2$ and origin at $b_2$. Then I want to express the second line segment in terms of the coordinate system {1} in the form of some translation and rotation. Commented Jun 8, 2021 at 14:08
• That's only possible, see @MichaelSeifert's comment, if the linesegments have same length! Commented Jun 8, 2021 at 14:16
• If I understand you correctly, then, you want $T(b_1) = b_2$ and $T(z_1) = z_2$? This will mean that $T(t_1) \neq t_2$, but you seem to be OK with that. Commented Jun 8, 2021 at 14:18
• I am not sure if I understand your question. Imagine moving from the world coordinate system to a new coordinate system such that the origin is at $b_1$ and $z$-axis is aligned with the vector $t_1-b_1$. How can we express the points $b_2$ and $t_2$ from this new coordinate frame? Commented Jun 8, 2021 at 14:24

If I understand your question right, you are looking for the transformation {b1,b1+z1}->{b2,b2+z2} ( not {b1,t1]->{b2,t2} )

FindGeometricTransformation finds this "rigid" transformation:

trafo=FindGeometricTransform[{b2,b2+z2},{b1,b1+z1}]
(*trafo[[2]][{b1,b1+z1}]=={b2,b2+z2} *)

M=TransformationMatrix[trafo[[2]]]
(*{{0., 1., 0., 0.}, {-1., 0., 0., 2.}, {0., 0., 1., -1.}, {0., 0., 0.,1.}}*)


Rotationmatrix

rot= M[[1;;3,1;;3]](*{{0., 1., 0.}, {-1., 0., 0.}, {0., 0., 1.}}*)


and translation

trans= M[[1 ;; 3, 4]](*{0., 2., -1.}*)


checking the transformation:

rot . b1 + trans == b2 (*True*)
rot . (b1 + z1) + trans == b2 + z2 (*True*)

• It says "FindGeometricTransform::insufficientPts: Need at least 2 non-degenerate pairs of points when estimating transformation class Automatic." on Mathematica 12.3 Commented Jun 8, 2021 at 14:31
• I tried plotting with the output you have. However, the figures don't match. Rota = {{0., -1., 0.}, {1., 0., 0.}, {0., 0., 1.}}; t21 = {2., 0., 1.}; Show[Graphics3D[{Cylinder[{{0, 0, 0}, {0, 0, l1}}, r1]}], Graphics3D[Cylinder[{t21, Rota . {0, 0, l2}}, r2]], Boxed -> False] Commented Jun 8, 2021 at 14:35
• My version v12.2 evaluates without problems. I have used your definitions b1,t1,z1,b2,t2,z2 Commented Jun 8, 2021 at 14:35
• Sorry, I modified the arguments of FindGeometricTransform, now it should work. Commented Jun 8, 2021 at 14:43
• I am sorry. They are not matching. Here is what I using to visualize:  Rota = {{0., -1., 0.}, {1., 0., 0.}, {0., 0., 1.}}; t21 = {2., 0., 1.}; Show[Graphics3D[{Cylinder[{{0, 0, 0}, {0, 0, l1}}, r1]}], Graphics3D[Cylinder[{t21, t21 + Rota . {0, 0, l2}}, r2]], Boxed -> False];  This looks different from the second figure in the question. Commented Jun 8, 2021 at 14:49