I want to know the angle of a hinge that I track in 3D space. There is a marker on each leg of the hinge which has its own coordinate system (T and F). One axis of each marker is aligned along the leg of the hinge so that it crosses and forms an angle - the hinge angle, $\theta$. I know the distance from the markers to the point where the axes cross (Lt and Lf).
I have the transformation data of each marker's origin relative to a global coordinate system located at the Position Sensor. I also have the quaternion orientation of each marker.
What process should I follow to get the hinge angle $\theta$ or, in other words, the angle between the 2 axes of the markers that intersect (in their local plane)?
Will appreciate any help! Thanks
Well, I do not see a reason you can't simply use the Law of cosine.
If I understand your question correctly, You can deduce the distance between the markers $d$ with simple Euclidean distance $d=\sqrt{(x_f-x_t)^2+(y_f-y_t)^2+(z_f-z_t)^2}$. From here, look at the triangle formed by that distance and the legs of the hinge as residing on the same plane. a simple reordering of the law of cosine will yield:
$$\theta=\cos^{-1}\left(\frac{(L_f)^2+(L_t)^2-d^2}{2L_fL_t}\right)$$$$=\cos^{-1}\left(\frac{(L_f)^2+(L_t)^2-(x_f-x_t)^2-(y_f-y_t)^2-(z_f-z_t)^2}{2L_fL_t}\right)$$