3D 笛卡尔空间中的变换

问题描述

我有一个顶点为 ABCDEFGH 在笛卡尔坐标系中的 3D 框。

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,-tx],[0,1,-ty],-tz],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],np.cos(angle),np.sin(angle),-np.sin(angle),1]])
    return np.matmul(rx,: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),[np.sin(angle),1]])
    return np.matmul(ry,:3]

def rotate_z(old_axis,np.ones(len(old_axis))]
    angle = np.deg2rad(angle)
    rz = np.array([[np.cos(angle),[-np.sin(angle),1]])
    return np.matmul(rz,: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.5707963267948966
            >>> angle_between((1,(1,0))
            0.0
            >>> angle_between((1,(-1,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,4],2,[2,0]])

old_cart = translate(old_cart)

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

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

rotx = rotate_y(old_cart,angx)

angy = np.rad2deg(angle_between(np.array([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,[-1.37705134,1.05499651,1.72936751],[-1.45574066,-0.94345489,[ 1.37705134,-1.05499651,-1.72936751],[ 1.45574066,0.94345489,[ 2.27160503,0.91133011,0.09637435],[ 2.19291571,-1.08712129,0.09637435]])

我想旋转盒子，使 old_cart[6] = [1,-2] 变成 new_cart[6] = [1,0]

所有距离都应该保持不变（刚体框）并且框的中心是固定的。

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

产生的输出（见第 7 行）

array([[-2.27160503,0.09637435]])

预期输出（见第 7 行）

array([[-2.27160503,0.00000,0.00000],0.09637435]])

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）