How do I calculate the WRF using a 3 point method?

Modified on Fri, 07 Jul 2023 at 04:57 PM

Let the coordinates of point P1 be x1, y1, z1. P1 will define the origin of the WRF.

P1= [x1,y1,z1]

Let the coordinates of point P2 be x2, y2, z2. P2 will lie on the positive x-axis of the WRF.

P2= [x2,y2,z2]

Let the coordinates of point P3 be x3, y3, z3. P3 will lie in quadrant 1 or 2 of the xy plane of the WRF.

P3= [x3,y3,z3]

We must find the unit vectors along the x, y and z axes of the WRF. These will be used to construct the rotation matrix that defines the orientation of the WRF with respect to the BRF. From this rotation matrix, we will use the equations that are presented in the Euler angles Tutorial to extract the Euler angles that define the orientation of the WRF.

ux = [x2 - x1, y2 - y1, z2 - z1];
norm_ux = sqrt(ux[0]^2 + ux[1]^2 + ux[2]^2);

if norm_ux < 10 mm,
     Message: "Invalid Data: Points P2 and P1 are too close to each other"
     exit
else
     uvx = ux/norm_ux;
endif

v13 = [x3 - x1, y3 - y1, z3 - z1];
norm_v13 = sqrt(v13[0]^2 + v13[1]^2 + v13[2]^2);

if norm_v13 < 10 mm,
    Message: "Invalid Data: Points P3 and P1 are too close to each other"
    exit
else 
     uv13 = v13/norm_v13;
endif

// uz = CrossProduct(uvx,uv13)
uz = [(uvx[1]*uv13[2])-(uvx[2]*uv13[1]), (uvx[2]*uv13[0])-(uvx[0]*uv13[2]), (uvx[0]*uv13[1])-(uvx[1]*uv13[0])];

// # the norm of this vector will be sin(theta), where theta is the angle between the two vectors, using the right-hand rule.
if !(norm_ux<10mm) and !(norm_v13 <10 mm)
     norm_uz = sqrt( uz[0]^2 + uz[1]^2 + uz[2]^2);
     uvz = uz/norm_uz;

// uvy = CrossProduct(uvz,uvx) 
uvy = [(uvz[1]*uvx[2])-(uvz[2]*uvx[1]), (uvz[2]*uvx[0])-(uvz[0]*uvx[2]), (uvz[0]*uvx[1)-(uvz[1]*uvx[0])] 

// R = [ uvx,  uvy, uvz], where uvx, uvy and uvz are column vectors
if abs(uvz[0]) == 1:
        beta_deg = uvz[0]*90
        gamma = arctan2(uvy[0],uvx[0])
        gamma_deg = rad2deg(gamma)
        alpha_deg = 0
else:
        beta = arcsin(uvz[0])
        beta_deg = rad2deg(beta)
        gamma = arctan2(-uvy[0],uvx[0])
        gamma_deg = rad2deg(gamma)
        alpha = arctan2(-uvz[1],uvz[2])
        alpha_deg = rad2deg(alpha) 
endif

result_WRF  = [ x1, y1, z1, alpha_deg, beta_deg, gamma_deg ]