Let me explain the problem. I have two different reference frames in a 3D space, one is the world origin and the other could be called as top frame.
The data:
- The relation between both frames position (as x, y, z) and orientation (as yaw, pitch, roll) in reference world. I called this variable $P = (x, y, z, yaw, pitch, roll)^T$
- The point $a = (a_x, a_y, a_z)^T$ with reference top.
- The point $b = (b_x, b_y, b_z)^T$ with reference origin.
The restriction:
- The vector magnitude between a and b must be in the range $(v_{min}, v_{max}) \in \mathbb{R}$.
So I would like to know the range from the point P (where the top frame was sitting) for each position direction and orientation rotation individually. Let me present what I have been doing for position, which works more or less fine.
The vector can be defined as:
$v = R \cdot a + (P_x, P_y, P_z)^T - b $
Where R is the rotation matrix:
$R = \begin{bmatrix} \cos\alpha\cos\beta & \cos\alpha\sin\beta\sin\gamma - \sin\alpha\cos\gamma & \cos\alpha\sin\beta\cos\gamma + \sin\alpha\sin\gamma \\ \sin\alpha\cos\beta & \sin\alpha\sin\beta\sin\gamma + \cos\alpha\cos\gamma & \sin\alpha\sin\beta\cos\gamma - \cos\alpha\sin\gamma \\ -\sin\beta & \cos\beta\sin\gamma & \cos\beta\cos\gamma \\ \end{bmatrix}$
And where $\alpha$, $\beta$ and $\gamma$ are yaw, pitch and roll respectly.
For example, the case of x direction I took:
$\Delta x = \pm\sqrt{v^2_{max} - (\|v\|^2 - v^2_x)} - v_x$
or
$\Delta x = \pm\sqrt{v^2_{min} - (\|v\|^2 - v^2_x)} - v_x$
And so on for y and z, but when it comes to the rotations, I am not able to get that range analytically... For now, I have been able to obtain the result by using Newton–Raphson iterative method but I would like to get it like the position because it's much faster computationally.
Let me know if I need to explain me better or if I need to provide anything else. Thanks in advance.
Lluis