I have a two points in the 3D coordinate space. Now, I want to send one of the points to the origin and make it (the line joining the two points) align with the X-axis and get the transformation matrix. The set of transformations that I do are:
offset = endPosition - startPosition
signx = offset.x >= 0 ? 1 : -1;
rz = -1 * signx * atan2(offset.y, sqrt((offset.z*offset.z) + (offset.x*offset.x))) * 180 / PI;
ry = atan2(offset.z, offset.x) * 180 / PI;
and then
transformationMatrix.translate(-startPosition.x, -startPosition.y, -startPosition.z)
transformationMatrix.rotate_z(rz);
transformationMatrix.rotate_y(ry);
and lastly,
endPoint = endPoint*transformationMatrix
Here, startPosition and endPosition are the two points.
Is this the right way of solving the problem? The problem is that when the initial line joining the points is almost perpendicular to the X-axis, I get some really weird results. Any idea where I could be going wrong?
To translate point $\vec{p}_0 = (x_0, y_0, z_0)$ to origin, rotating point $\vec{p}_1 = (x_1, y_1, z_1)$ to $x$-axis (to $\left (\sqrt{(x_1-x_0)^2+(y_1-y_0)^2+(z_1-z_0)^2}, 0, 0\right)$), use transformation matrix $$M = \left [ \begin{matrix} \frac{x_1-x_0}{d} & \frac{y_1-y_0}{d} & \frac{z_1-z_0}{d} & \frac{x_0(x_0-x_1) + y_0(y_0-y_1) + z_0(z_0-z_1)}{d} \\ \frac{y_0-y_1}{r} & \frac{x_1-x_0}{r} & 0 & \frac{x_0 y_1 - x_1 y_0}{r} \\ \frac{(z_1 - z_0)(x_0 - x_1)}{r \; d} & \frac{(z_1 - z_0)(y_0 - y_1)}{r \; d} & \frac{r}{d} & \frac{(z_1 - z_0) \left ( (x_1 - x_0) x_0 + (y_1 - y_0) y_0 \right )}{r \; d} - \frac{r z_0}{d} \end{matrix} \right ]$$ where $$\begin{align} d &= \sqrt{(x_1-x_0)^2 + (y_1-y_0)^2 + (z_1-z_0)^2}\\ r &= \sqrt{(x_1-x_0)^2 + (y_1-y_0)^2} \end{align}$$
The question whether OP is performing this transformation is difficult to say, because none of the math actually performed is shown. If OP's results only work in some cases and not others, it means something is wrong. Whether that is OP's calculations or the algebra toolkit used, is impossible to say without further details.
There are several options, but the easiest is to do it in three steps: Translate $\vec{p}_0$ to origin, rotate $xy$-plane (around $z$-axis, to bring $y_1$ after rotation to zero), and rotate $xz$-plane (around $y$-axis, to bring $z_1$ after rotation to zero).
The transformation matrix we need has a pure rotation part (first three columns), and a post-rotation translation part (fourth column), essentially $$M = \left [ \begin{matrix} X_x & Y_x & Z_x & T_x \\ X_y & Y_y & Z_y & T_y \\ X_z & Y_z & Z_z & T_z \end{matrix} \right ]$$ so that translating point $\vec{p} = ( x, y, z )$ yields $$\vec{p}' = M \vec{p} = \left [ \begin{matrix} X_x x + Y_x y + Z_x z + Tx \\ X_y x + Y_y y + Z_y z + Ty \\ X_z x + Y_z y + Z_z z + Tz \end{matrix} \right ]$$ Note that we need our translation to occur before the rotation. Fortunately, converting a pre-rotation translation to post-rotation translation is trivial: just multiply the rotation matrix and the pre-rotation translation to get the post-rotation translation, $\vec{t}_{POST} = R\vec{t}_{PRE}$.
The pure rotation part has some very nice properties. It is an orthogonal matrix, whose columns and rows are orthonormal vectors. (This means the dot product between any two row vectors or any two column vectors is zero, and the dot product of any row vector and itself, or any column vector and itself, is 1.)
Most interestingly, its transpose is also its inverse, and that's what we use here: $$R = \left [ \begin{matrix} X_x & Y_x & Z_x \\ X_y & Y_y & Z_y \\ X_z & Y_z & Z_z \end{matrix} \right ], \quad R^T = R^{-1} = \left [ \begin{matrix} X_x & X_x & X_x \\ Y_y & Y_y & Y_y \\ Z_z & Z_z & z_z \end{matrix} \right ]$$ (Note that this also means that converting a post-rotation translation vector to a pre-rotation translation is just as easy as before; just use the transpose of the rotation matrix: $\vec{t}_{PRE} = R^T\vec{t}_{POST}$.)
Simply put, the first row of $R$ is an unit vector, showing the direction that will be rotated to $x$ axis. The second row is an unit vector that shows the direction that will be rotated to $y$ axis, and the third the unit vector that will be rotated to $z$ axis. All we need to do, is to construct these three orthonormal vectors.
Constructing those directly in 3D is difficult, because we only know one -- there are infinite number of options to choose from.
In 2D, constructing an orthogonal vector is simple: we just rotate it by 90 degrees, so that $(x,y)$ becomes $(-y,x)$. Since two consecutive rotations can be combined by multiplying the rotation matrices (first rotation rightmost, last rotation leftmost), we can do two planar rotations, and combine them, to obtain our needed rotation matrix. Since it will be a pure rotation, we can then calculate our post-rotation translation as $R\vec{p}_0$.
To extend a 2D rotation to 3D, just insert it into an identity matrix. (That is, insert zeros for other elements, except one at diagonals.)
The first rotation must rotate $(x,y)$ to $(\sqrt{x^2+y^2},0) = (r,0)$. In other words, the new $x$ axis is $(x/r,y/r)$ in the original coordinate system, and the new $y$ axis is $(-y/r,x/r)$ in the original coordinate system. Thus, the rotation matrix is $$R_1 = \left [ \begin{matrix} \frac{x}{r} & \frac{y}{r} & 0 \\ \frac{-y}{r} & \frac{x}{r} & 0 \\ 0 & 0 & 1 \end{matrix} \right ]$$ After rotation $R_1$, our original vector $(x,y,z)$ is now at $(\sqrt{x^2+y^2}, 0, z) = (r,0,z)$, so we need another rotation that brings $(r/d,0,z/d)$ to $x$-axis (to $(d,0,0)$; and therefore $(-z/d,0,r/d)$ to $z$-axis): $$R_2 = \left [ \begin{matrix} \frac{r}{d} & 0 & \frac{z}{d} \\ 0 & 1 & 0 \\ \frac{-z}{d} & 0 & \frac{r}{d} \end{matrix} \right ]$$ Combining the two rotations yields us the rotation part of the transformation matrix initially shown, $$R = R_2 R_1 = \left [ \begin{matrix} \frac{x}{d} & \frac{y}{d} & \frac{z}{d} \\ \frac{y}{r} & \frac{x}{r} & 0 \\ \frac{-z x}{r \; d} & \frac{-z y}{r \; d} & \frac{r}{d} \end{matrix} \right ]$$ except with $(x,y,z)$ considered after the required translation, ie. $x = x_1 - x_0$, $y = y_1 - y_0$, and $z = z_1 - z_0$.
Remember that the order of the rotations matters!
Since we need the post-rotation translation vector $\vec{t}$ for the transformation matrix, and we know the pre-rotation vector is $-\vec{p}_0 = (-x_0, -y_0, -z_0)$, we can simply rotate it by $R$ to obtain the post-translation vector: $$\vec{t} = R ( -\vec{p}_0 ) = \left [ \begin{matrix} -\frac{x x_0 + y y_0 + z z_0}{d} \\ \frac{y x_0 - x y_0}{r} \\ \frac{z x x_0 + z y y_0}{r \; d} - \frac{r z_0}{d} \end{matrix} \right ]$$ Again, substituting $x = x_1 - x_0$, $y = y_1 - y_0$, and $z = z_1 - z_0$ yields the post-translation vector in the original transformation matrix shown.