Search code examples
math3dprojectionquaternionsangle

Calculate enclosing vertical pitch angle of a box in a coordinate system


Illustration of the problem

Given a coordinate system (green - which could sit at the center of a laser sensor) and a box with dimensions l, w, h and position of center c (c_x, c_y, c_z) and its orientation as a quaternion (q_x, q_y, q_z, q_w) in that coordinate system I want to find the minimal enclosing pitch angles (red - from the perspective of the coordinate system) of a box β_max and β_min.

This question seems to be very tricky, because how do I know which is the "highest and lowest point" from the perspective of the coordinate system? It obviously depends on the orientation and and the dimensions of the box: it could be the edge of the box or just the corner, but I have no mathematical way of expressing it.


Solution

  • The first box vertex has coordinates relative to box center (-w/2, -h/2, -l/2).

    Rotate this point using quaternion, add result to vector (cx,cy,cz), get vector V0=(v0x, v0y, v0z)

    Angle between this vector and OZ axis using vector length is (through so-called direction cosine)

    A0 = acos(V0.dot.(0,0,1) / len(V0)) = acos(v0z / len(V0))
    

    Angle between this vector and OXY plane (as far as I understand, you need namely this angle) :

    B0 = Pi/2 - A0
    

    Calculate angles Bi for all box vertices (changing signs in the first line formula) and choose max an min values.