How to get global coordinate of point in a Parent-Child system

19 Views Asked by At

I have been looking this up for the past few days and have tried many different ways of accomplishing my goal but I fear that I may be on the right path but without the sufficient knowledge to get where I want to be.

I have coordinates in a parent-child system where the child is in the parent's coordinate system. It starts with a object with it's midpoint at (0,0,0) and objects build outwards. Every object that gets placed off of it's "parent" assumes that it's (0,0,0) is at it's parent object's midpoint. Each object has rotation, so the local coordinate system is rotated in relation to the parent's rotation. So in essence, there are multiple nest parent-child systems that all branch and start from (0,0,0) in the world coordinates from the first object.

After much googling and view some 3d rendering matrices, I have landed where I am currently. I have transformation matrices that look like this:

x1, x2, x3, t1, ---> x axis

y1, y2, y3, t2, ---> y axis

z1, z2, z3, t3, ---> z axis

 0,  0,  0,  1  ---> w (w = 1) 

Where I have the x,y,z being the rotation matrices, t is the x,y,z translation (or my x,y,z coords) with 0,0,0 at midpoint of parent.

My current code:

# Rotation of parent 
thetax = 0
thetay = np.deg2rad(-81.047)
phi = np.deg2rad(44.989)

# Rotation matrix definitions
Rx = np.array([[1, 0, 0],
               [0, np.cos(thetax), -np.sin(thetax)],
               [0, np.sin(thetax), np.cos(thetax)]])

Ry = np.array([[np.cos(thetay), 0, np.sin(thetay)],
               [0, 1, 0],
               [-np.sin(thetay), 0, np.cos(thetay)]])

Rz = np.array([[np.cos(phi), -np.sin(phi), 0],
               [np.sin(phi), np.cos(phi), 0],
               [0, 0, 1]])

# Getting the euler rotation matrix where you rotated X, then Y, then Z ('XYZ') rotation
Rxyz = np.dot(Rz, np.dot(Ry, Rx))

parentloc = np.array([13.34, 9.452, 77.66]) # location of parent (already in global coordinates since it's origin is at origin)
childloc = np.array([78.116, -0.101, 27.203]) # location of child in the parent's rotated coordinate system
# Parent Transformation Matrix
parent_tmatrix = np.eye(4)
parent_tmatrix[0:3, 3] = np.array([13.34, 9.452, 77.66]) # Location of parent 

# Child Transformation Matrix 
child_tmatrix = np.eye(4)
child_tmatrix[0:3, 3] = childloc
child_tmatrix[0:3, 0:3] = Rxyz

# global location calculation x,y,z = parent_tmatrix * child_tmatrix * location
global_loc = np.matmul(parent_tmatrix, child_tmatrix) 
print(global_loc[:3, 3])  # Extract only the x, y, z coordinates

expected = np.array([87.5955, 45.6619, 81.7937])
print(expected) 

Currently I get different answers between my expected and what I actually calculate. I have a feeling that the way I am defining my transformation matrices is wrong in some way but I have yet to find a combination that works correctly. Any helpful insight into how to do this efficiently would be greatly appreciated. I have to have a standalone code that calculates this manually, previously I would have just used blender (which is where I get my expected) but I am not able to do for various reasons. Most of the answers I have googled/researched have not yielded any useful results as they seem to either be slightly different or I am not comprehending correctly! Thanks in advance.

0

There are 0 best solutions below