def rot_top(self, angle):
rot = rm.rotmat_from_axangle((0, 0, 1), angle / 180 * np.pi)
mat = da.npv3mat3_to_pdmat4((0, 0, 0), rot)
for i, cube in enumerate(self.node_list):
if round(cube.getPos()[2], 2) == 0.02:
print(i)
self.node_list[i].setMat(
da.npmat4_to_pdmat4(
np.dot(da.pdmat4_to_npmat4(mat),
da.pdmat4_to_npmat4(
self.node_list[i].getMat()))))
def sethomomat(self, homomat):
"""
set the pose of the dynamic body
:param homomat: the homomat of the original frame (the collision model)
:return:
author: weiwei
date: 2019?, 20201119
"""
pos = rm.homomat_transform_points(homomat, self.com)
rotmat = homomat[:3, :3]
self.setTransform(TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))
def set_homomat(self, homomat):
"""
set the pose of the dynamic body
:param homomat: the homomat of the original frame (the collision model)
:return:
author: weiwei
date: 2019?, 20201119
"""
tmp_homomat = copy.deepcopy(homomat)
tmp_homomat[:3, 3] = tmp_homomat[:3, 3] * base.physics_scale
pos = rm.homomat_transform_points(tmp_homomat, self.com)
rotmat = tmp_homomat[:3, :3]
self.setTransform(
TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))