My problem is the following: I want to get the spherical coordinates of a point B in the frame of a point A. I have access to the cartesian world coordinates of the point A, the cartesian world coordinates of the point B, and to a quaternion describing the rotation of the point A with regards to the world frame of reference (a so-called "orientation quaternion" or "attitude quaternion").
So far, this is the kind of code I use (in OpenGL math library, glm):
#define U glm::dvec3(1.0, 0.0, 0.0)
#define V glm::dvec3(0.0, 1.0, 0.0)
#define W glm::dvec3(0.0, 0.0, 1.0)
glm::dvec3 cart_to_local(glm::dvec3 B_position) {
glm::dvec3 e1 = A_rotation_quaternion * U;
glm::dvec3 e2 = A_rotation_quaternion * V;
glm::dvec3 e3 = A_rotation_quaternion * W;
glm::mat3 transition_matrix(e1.x, e1.y, e1.z, e2.x, e2.y, e2.z, e3.x, e3.y, e3.z);
glm::mat3 reverse_transition_matrix = glm::inverse(transition_matrix);
glm::dvec3 v = B_position - A_position;
return reverse_transition_matrix * v;
}
I then translate the cartesian coordinates to the spherical coordinates using well-known formulas (featuring acos, atan2, and so on).
It works, but to me it looks like there are too many steps involved. Is there a way to make this code "smaller" by using more standards functions that I'm not aware of?