Python 三维笛卡尔空间中的变换

在笛卡尔坐标系中，我有一个顶点为ABCDEFGH的三维长方体

import numpy as np
import math as m
import pdb
from scipy import ndimage
import torch
import matplotlib.pyplot as plt

def cart2sph(x,y,z):
    XsqPlusYsq = x**2 + y**2
    r = np.sqrt(XsqPlusYsq + z**2)               # r
    elev = np.arccos(z/r)                        # theta
    az = np.arctan2(y,x)                         # phi
    return np.c_[r, elev, az]

def translate(old_axis):
    old_axis = np.c_[old_axis, np.ones(len(old_axis))]
    tx, ty, tz, _ = old_axis.mean(axis=0)
    tsl = np.array([[1,0,0,-tx], [0,1,0,-ty], [0,0,1,-tz], [0,0,0,1]])
    return np.matmul(tsl, old_axis.T).T[:,:3]

def rotate_x(old_axis, angle):
#     pdb.set_trace()
    old_axis = np.c_[old_axis, np.ones(len(old_axis))]
    angle = np.deg2rad(angle)
    rx = np.array([[1,0,0,0], [0, np.cos(angle), np.sin(angle), 0], [0, -np.sin(angle), np.cos(angle), 0], [0,0,0,1]])
    return np.matmul(rx, old_axis.T).T[:,:3]

def rotate_y(old_axis, angle):
    old_axis = np.c_[old_axis, np.ones(len(old_axis))]
    angle = np.deg2rad(angle)
    ry = np.array([[np.cos(angle), 0, -np.sin(angle), 0], [0, 1, 0, 0], [np.sin(angle), 0, np.cos(angle), 0], [0, 0, 0, 1]])
    return np.matmul(ry, old_axis.T).T[:,:3]

def rotate_z(old_axis, angle):
    old_axis = np.c_[old_axis, np.ones(len(old_axis))]
    angle = np.deg2rad(angle)
    rz = np.array([[np.cos(angle), np.sin(angle), 0, 0], [-np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
    return np.matmul(rz, old_axis.T).T[:,:3]

def unit_vector(vector):
    """ Returns the unit vector of the vector.  """
    return vector / np.linalg.norm(vector)

def angle_between(v1, v2):
    """ Returns the angle in radians between vectors 'v1' and 'v2'::

            >>> angle_between((1, 0, 0), (0, 1, 0))
            1.5707963267948966
            >>> angle_between((1, 0, 0), (1, 0, 0))
            0.0
            >>> angle_between((1, 0, 0), (-1, 0, 0))
            3.141592653589793
    """
    v1_u = unit_vector(v1)
    v2_u = unit_vector(v2)
    return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0))

old_cart = np.array([[0,0,4], [0,2,4], [2,2,4], [2,0,4], [0,0,0], [0,2,0], [2,2,0], [2,0,0]])

old_cart = translate(old_cart)

print(old_cart)
array([[-1., -1.,  2.],
       [-1.,  1.,  2.],
       [ 1.,  1.,  2.],
       [ 1., -1.,  2.],
       [-1., -1., -2.],
       [-1.,  1., -2.],
       [ 1.,  1., -2.],
       [ 1., -1., -2.]])

angx = np.rad2deg(angle_between(np.array([0,1,0]), old_cart[6]))
# angx = np.rad2deg(angle_between(np.array([0,1,0]), old_cart[6]))
print(angx, np.deg2rad(angx))
# np.rad2deg(angle_between(old_cart[6], np.array([0,1,0])))

rotx = rotate_y(old_cart, angx)

angy = np.rad2deg(angle_between(np.array([0,0,1]), rotx[6]))
print(angy)
# roty = rotate_z(rotx, -angy)
roty = rotate_z(rotx, 90-angy)

print(roty)

array([[-2.27160503, -0.91133011, -0.09637435],
       [-2.19291571,  1.08712129, -0.09637435],
       [-1.37705134,  1.05499651,  1.72936751],
       [-1.45574066, -0.94345489,  1.72936751],
       [ 1.37705134, -1.05499651, -1.72936751],
       [ 1.45574066,  0.94345489, -1.72936751],
       [ 2.27160503,  0.91133011,  0.09637435],
       [ 2.19291571, -1.08712129,  0.09637435]])

我想旋转该框，使

旧购物车[6]=[1,1，-2]

变成

新购物车[6]=[1,0,0]

所有距离应保持不变（刚性长方体），长方体的中心是固定的

问题是，即使在旋转后，最后第二行的y和z坐标也不会变为0.0000

产出（见第7行）

预期输出（见第7行）

array([[-2.27160503, -0.91133011, -0.09637435],
       [-2.19291571,  1.08712129, -0.09637435],
       [-1.37705134,  1.05499651,  1.72936751],
       [-1.45574066, -0.94345489,  1.72936751],
       [ 1.37705134, -1.05499651, -1.72936751],
       [ 1.45574066,  0.94345489, -1.72936751],
       [ 2.27160503,  0.91133011,  0.09637435],
       [ 2.19291571, -1.08712129,  0.09637435]])

array([[-2.27160503, -0.91133011, -0.09637435],
       [-2.19291571,  1.08712129, -0.09637435],
       [-1.37705134,  1.05499651,  1.72936751],
       [-1.45574066, -0.94345489,  1.72936751],
       [ 1.37705134, -1.05499651, -1.72936751],
       [ 1.45574066,  0.94345489, -1.72936751],
       [ 2.27160503,  0.00000,  0.00000],
       [ 2.19291571, -1.08712129,  0.09637435]])