def translation_of(frame):
"""
:param frame: 4x4 Matrix
:type frame: Matrix
:return: 4x4 Matrix; sets the rotation part of a frame to identity
:rtype: Matrix
"""
return se.eye(3).col_join(se.Matrix([[0] * 3])).row_join(frame[:4, 3:])
def inverse_frame(frame):
"""
:param frame: 4x4 Matrix
:type frame: Matrix
:return: 4x4 Matrix
:rtype: Matrix
"""
inv = se.eye(4)
inv[:3, :3] = frame[:3, :3].T
inv[:3, 3] = -inv[:3, :3] * frame[:3, 3]
return inv
def translation3(x, y, z):
"""
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:type z: Union[float, Symbol]
:return: 4x4 Matrix
[[1,0,0,x],
[0,1,0,y],
[0,0,1,z],
[0,0,0,1]]
:rtype: Matrix
"""
r = se.eye(4)
r[0, 3] = x
r[1, 3] = y
r[2, 3] = z
return r
def inverse_frame(frame):
mf = GM if type(frame) == GM else se.Matrix
inv = mf(se.eye(4))
inv[:3, :3] = frame[:3, :3].T
inv[:3, 3] = -inv[:3, :3] * frame[:3, 3]
return inv
def trans_of(frame):
return se.eye(3).col_join(se.Matrix([[0] * 3])).row_join(frame[:4, 3:])
def translation3(point):
return sp.eye(3).col_join(sp.Matrix([[0] * 3])).row_join(point)