Is there a library or function for calculating distance/angles between models?

edit

I need to calculate the real, not estimated, distance and angles between two models in Gazebo. I need to know what angle the second model is in the z and y direction from the first model, using the first models orientation.

To better explain with an example of what I'm doing, if I have a quadcopter with a camera facing in any direction, and there is another model in the camera, what is the real angle between where the quadcopter is currently facing and the object in the left/right direction of the camera and what is the real angle between where the quadcopter is currently facing and the object in the up/down direction of the camera.

This is for comparison purposes to angles estimated from the image taken from the camera. I need them to be as close to exact as possible.

I was hoping there was already some package or functions for this. I looked at the gazebo/math directory, and I see some functions there to help with the calculations, but not one that does the complete calculation of angles/distances between two given positions/orientations.

So, are there any packages/functions that do that? Or, will I need to figure out the calculations from scratch?

Thanks.