将正方形旋转为矢量

时间:2017-06-08 16:58:33

标签: python numpy vector 3d rotation

Win 7,x64,Python 2.7

我正在尝试旋转最初位于xz平面中的正方形,以使其正常与给定的3D矢量对齐。此外,我正在将方块转换为向量的开头,但这不是问题。

我采取的路径如下,

1)通过给定矢量&的交叉乘积找到旋转轴。正方形的法线,在这种情况下是y方向的单位向量。

2)通过给定矢量的点积和平方的法线找到旋转角度。

3)建立适当的旋转矩阵。

4)将旋转矩阵应用于正方形的顶点。

5)转换到给定矢量的开头。

代码..

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
na = np.array

def rotation_matrix(axis, theta):
    """
    Return the rotation matrix associated with counterclockwise rotation about
    the given axis by theta radians.
    """
    axis = np.asarray(axis)
    axis = axis/math.sqrt(np.dot(axis, axis))
    a = math.cos(theta/2.0)
    b, c, d = -axis*math.sin(theta/2.0)
    aa, bb, cc, dd = a*a, b*b, c*c, d*d
    bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
    return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
                     [2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
                     [2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])


edgeLen = 4.0                 # length of square side
pos = na([2.0,2.0,2.0])       # starting point of vector
dirc = na([6.0,6.0,6.0])      # direction of vector

Ux = na([1.0,0.0,0.0])      # unit basis vectors
Uy = na([0.0,1.0,0.0])
Uz = na([0.0,0.0,1.0])

x = pos[0]
y = pos[1]
z = pos[2]

# corner vertices of square in xz plane
verts = na([[edgeLen/2.0, 0, edgeLen/2.0], 
            [edgeLen/2.0, 0, -edgeLen/2.0], 
            [-edgeLen/2.0, 0, -edgeLen/2.0], 
            [-edgeLen/2.0, 0, edgeLen/2.0]])

# For axis & angle of rotation
dirMag = np.linalg.norm(dirc)
axR = np.cross(dirc, Uy)
theta = np.arccos((np.dot(dirc, Uy) / dirMag))

Rax = rotation_matrix(axR, theta)   # rotation matrix

# rotate vertices
rotVerts = na([0,0,0])

for v in verts:

    temp = np.dot(Rax, v)
    temp = na([temp[0]+x, temp[1]+y, temp[2]+z])
    rotVerts = np.vstack((rotVerts, temp))

rotVerts = np.delete(rotVerts, rotVerts[0], axis=0)

# plot
# oringinal square
ax.scatter(verts[:,0], verts[:,1], verts[:,2], s=10, c='r', marker='o')
ax.plot([verts[0,0], verts[1,0]], [verts[0,1], verts[1,1]], [verts[0,2], verts[1,2]], color='g', linewidth=1.0)
ax.plot([verts[1,0], verts[2,0]], [verts[1,1], verts[2,1]], [verts[1,2], verts[2,2]], color='g', linewidth=1.0)
ax.plot([verts[2,0], verts[3,0]], [verts[2,1], verts[3,1]], [verts[2,2], verts[3,2]], color='g', linewidth=1.0)
ax.plot([verts[0,0], verts[3,0]], [verts[0,1], verts[3,1]], [verts[0,2], verts[3,2]], color='g', linewidth=1.0)

# rotated & translated square
ax.scatter(rotVerts[:,0], rotVerts[:,1], rotVerts[:,2], s=10, c='b', marker='o')
ax.plot([rotVerts[0,0], rotVerts[1,0]], [rotVerts[0,1], rotVerts[1,1]], [rotVerts[0,2], rotVerts[1,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[1,0], rotVerts[2,0]], [rotVerts[1,1], rotVerts[2,1]], [rotVerts[1,2], rotVerts[2,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[2,0], rotVerts[3,0]], [rotVerts[2,1], rotVerts[3,1]], [rotVerts[2,2], rotVerts[3,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[0,0], rotVerts[3,0]], [rotVerts[0,1], rotVerts[3,1]], [rotVerts[0,2], rotVerts[3,2]], color='b', linewidth=1.0)

# vector
ax.plot([pos[0], pos[0]+dirc[0]], [pos[1], pos[1]+dirc[1]], [pos[1], pos[1]+dirc[1]], color='r', linewidth=1.0)

ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

这给出了以下输出.. enter image description here

绿色正方形是xz平面中的原始正方形,蓝色正方形是转换后的正方形&给定的矢量为红色。

你可以看到它很好。经过几个小时的倾诉,我遇到了类似的问题。回答,我仍然不知道为什么这不起作用。

那我在这里错过了什么?

编辑:在El Dude在下面的评论中提出的Euler Angles link之后,我尝试了以下内容......

定义参考xyz的静态帧的yz平面中的平方,其具有基矢量Ux,Uy&乌兹

使用方向矢量'dirVec'作为我想要将平方旋转到的平面的法线。

我决定使用欧洲角度链接中描述的x-convention和ZXZ旋转矩阵。

我采取的步骤,

1)使用Tx,Ty&创建一个旋转的帧。 Tz作为基础向量;

Tx = dirVec
Ty = Tx cross Uz (Tx not allowed to parallel to Uz)
Tz = Ty cross Tx

2)定义节点线,沿着平面UxUy&的交叉点的矢量。 TxTy采用Uz和U的交叉产品TZ

3)根据上述链接

中的定义定义欧拉角

4)根据上述链接

定义ZXZ旋转矩阵

5)将旋转矩阵应用于方形顶点的坐标

它不起作用,发生了奇怪的事情,无论'dirVec'alpha的值总是为0。

是否有一些明显的事情我只是缺席了?

这是修改后的代码......

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
na = np.array

def rotation_ZXZ(alpha=0.0, beta=0.0, gamma=0.0):
    """
    Return ZXZ rotaion matrix
    """  
    a = alpha
    b = beta
    g = gamma

    ca = np.cos(a)
    cb = np.cos(b)
    cg = np.cos(g)

    sa = np.sin(a)
    sb = np.sin(b)
    sg = np.sin(g)

    return np.array([[(ca*cg-cb*sa*sg), (-ca*sg-cb*cg*sa), sa*sb],
                     [(cg*sa+ca*cb*sg), (ca*cb*cg-sa*sg), -ca*sb],
                     [sb*sg, cg*sb, cb]])

def rotated_axes(vector=[0,1,0]):
    """
    Return unit basis vectors for rotated frame
    """
    vx = np.asarray(vector) / np.linalg.norm(vector)

    if vx[1] != 0 or vx[2] != 0: 
        U = na([1.0, 0.0, 0.0])
    else:
        U = na([0.0, 1.0, 0.0])

    vz = np.cross(vx, U)
    vz = vz / np.linalg.norm(vz)
    vy = np.cross(vx, vz)
    vy = vy / np.linalg.norm(vy)

    vx = bv(vx[0], vx[1], vx[2])
    vy = bv(vy[0], vy[1], vy[2])
    vz = bv(vz[0], vz[1], vz[2])

    return vx, vy, vz

def angle_btw_vectors(v1=[1,0,0], v2=[0,1,0]):
    """
    Return the angle, in radians, between 2 vectors
    """
    v1 = np.asarray(v1)
    v2 = np.asarray(v2)
    mags = np.linalg.norm(v1) * np.linalg.norm(v2)

    return np.arccos(np.dot(v1, v2) / mags)

edgeLen = 4.0                 # length of square side
dirVec = na([4,4,4])          # direction of given vector
pos = na([0.0, 0.0, 0.0])     # starting point of given vector
x = pos[0]
y = pos[1]
z = pos[2] 

Ux = na([1,0,0])              # Unit basis vectors for static frame
Uy = na([0,1,0])
Uz = na([0,0,1])   

Tx, Ty, Tz = rotated_axes(dirVec) # Unit basis vectors for rotated frame
                                  # where Tx = dirVec / |dirVec|

nodeLine = np.cross(Uz, Tz) # Node line - xy intersect XY

alpha = angle_btw_vectors(Ux, nodeLine)     #Euler angles
beta = angle_btw_vectors(Uz, Tz)
gamma = angle_btw_vectors(nodeLine, Tx)

Rzxz = rotation_ZXZ(alpha, beta, gamma)     # Rotation matrix

print '--------------------------------------'
print 'Tx: ', Tx
print 'Ty: ', Ty
print 'Tz: ', Tz
print 'Node line: ', nodeLine
print 'Tx.dirVec: ', np.dot(Tx, (dirVec / np.linalg.norm(dirVec)))
print 'Ty.dirVec: ', np.dot(Ty, dirVec)
print 'Tz.dirVec: ', np.dot(Tz, dirVec)
print '(Node Line).Tx: ', np.dot(Tx, nodeLine)
print 'alpha: ', alpha * 180 / np.pi
print 'beta: ', beta * 180 / np.pi
print 'gamma: ', gamma * 180 / np.pi
#print 'Rzxz: ', Rxzx

# corner vertices of square in yz plane
verts = na([[0, edgeLen/2.0, edgeLen/2.0], 
            [0, edgeLen/2.0, -edgeLen/2.0], 
            [0, -edgeLen/2.0, -edgeLen/2.0], 
            [0, -edgeLen/2.0, edgeLen/2.0]])

rotVerts = na([0,0,0])
for v in verts:

    temp = np.dot(Rzxz, v)
    temp = na([temp[0]+x, temp[1]+y, temp[2]+z])
    rotVerts = np.vstack((rotVerts, temp))

rotVerts = np.delete(rotVerts, rotVerts[0], axis=0)


# plot
# oringinal square
ax.scatter(verts[:,0], verts[:,1], verts[:,2], s=10, c='g', marker='o')
ax.plot([verts[0,0], verts[1,0]], [verts[0,1], verts[1,1]], [verts[0,2], verts[1,2]], color='g', linewidth=1.0)
ax.plot([verts[1,0], verts[2,0]], [verts[1,1], verts[2,1]], [verts[1,2], verts[2,2]], color='g', linewidth=1.0)
ax.plot([verts[2,0], verts[3,0]], [verts[2,1], verts[3,1]], [verts[2,2], verts[3,2]], color='g', linewidth=1.0)
ax.plot([verts[0,0], verts[3,0]], [verts[0,1], verts[3,1]], [verts[0,2], verts[3,2]], color='g', linewidth=1.0)

# rotated & translated square
ax.scatter(rotVerts[:,0], rotVerts[:,1], rotVerts[:,2], s=10, c='b', marker='o')
ax.plot([rotVerts[0,0], rotVerts[1,0]], [rotVerts[0,1], rotVerts[1,1]], [rotVerts[0,2], rotVerts[1,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[1,0], rotVerts[2,0]], [rotVerts[1,1], rotVerts[2,1]], [rotVerts[1,2], rotVerts[2,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[2,0], rotVerts[3,0]], [rotVerts[2,1], rotVerts[3,1]], [rotVerts[2,2], rotVerts[3,2]], color='b', linewidth=1.0)
ax.plot([rotVerts[0,0], rotVerts[3,0]], [rotVerts[0,1], rotVerts[3,1]], [rotVerts[0,2], rotVerts[3,2]], color='b', linewidth=1.0)

# Rotated reference coordinate system
ax.plot([pos[0], pos[0]+Tx[0]], [pos[1], pos[1]+Tx[1]], [pos[2], pos[2]+Tx[2]], color='r', linewidth=1.0)
ax.plot([pos[0], pos[0]+Ty[0]], [pos[1], pos[1]+Ty[1]], [pos[1], pos[2]+Ty[2]], color='b', linewidth=1.0)
ax.plot([pos[0], pos[0]+Tz[0]], [pos[1], pos[1]+Tz[1]], [pos[1], pos[2]+Tz[2]], color='g', linewidth=1.0)

ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

1 个答案:

答案 0 :(得分:1)

这是我提出的解决方案 - 虽然没有大量的测试,但它应该可行。解决方案更为通用,因为它适用于任何方向的任何2D对象,您必须调整的唯一事项是存储在obj中的顶点(这可以做得更好但在这里我只是创建了一个列表手工点)。

注意,我将mObj定义为对象的“中心” - 这不会改变功能,但却是显示的法线向量的锚点。

以下是数学的一些解释: 我们需要做的是找到正确的旋转轴和角度,这样我们只需要一个矩阵乘法(原则上你可以使用欧拉角,这将是一个等价的解决方案)。角度很容易,因为它是由点积产生的:

  

dot(a,b)= | a | | B | * cos(theta)

其中θ是向量a和b之间的角度。要找到旋转轴,我们可以使用由a和b跨越的平面的法向量,即使用叉积并对其进行标准化:

  

rotAxis = cross(a,b)/ | cross(a,b)|

请注意,此向量与a和b正交,因此我们正在寻找轴。

希望这有帮助。

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def rotateVector3D(v, theta, axis):
    """ Takes a three-dimensional vector v and rotates it by the angle theta around the specified axis.
    """
    return np.dot(rotationMatrix3D(theta, axis), v)


def rotationMatrix3D(theta, axis):
    """ Return the rotation matrix associated with counterclockwise rotation about
        the given axis by theta radians.
    """
    axis = np.asarray(axis) / np.sqrt(np.dot(axis, axis)) 
    a = np.cos(theta/2.0)
    b, c, d = -axis*np.sin(theta/2.0)
    aa, bb, cc, dd = a**2, b**2, c**2, d**2
    bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
    return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
                     [2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
                     [2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])


def drawObject(ax, pts, color="red"):
    """ Draws an object on a specified 3D axis with points and lines between consecutive points.
    """
    map(lambda pt: ax.scatter(*pt, s=10, color=color), pts)
    for k in range(len(pts)-1):
        x, y, z = zip(*pts[k:k+2])
        ax.plot(x, y, z, color=color, linewidth=1.0)
    x, y, z = zip(*[pts[-1],pts[0]])
    ax.plot(x, y, z, color=color, linewidth=1.0)


def normalVector(obj):
    """ Takes a set of points, assumed to be flat, and returns a normal vector with unit length.
    """
    n = np.cross(np.array(obj[1])-np.array(obj[0]), np.array(obj[2])-np.array(obj[0]))
    return n/np.sqrt(np.dot(n,n))


# Set the original object (can be any set of points)
obj = [(2, 0, 2), (2, 0, 4), (4, 0, 4), (4, 0, 2)]
mObj = (3, 0, 3)
nVecObj = normalVector(obj)

# Given vector.
vec = (6, 6, 6)

# Find rotation axis and angle.
rotAxis = normalVector([(0,0,0), nVecObj, vec])
angle =  np.arccos(np.dot(nVecObj, vec) / (np.sqrt(np.dot(vec, vec)) * np.sqrt(np.dot(nVecObj, nVecObj))))
print "Rotation angle: {:.2f} degrees".format(angle/np.pi*180)


# Generate the rotated object.
rotObj = map(lambda pt: rotateVector3D(pt, angle, rotAxis), obj)
mRotObj = rotateVector3D(mObj, angle, rotAxis)
nVecRotObj = normalVector(rotObj)


# Set up Plot.
fig = plt.figure()
fig.set_size_inches(18,18)
ax = fig.add_subplot(111, projection='3d')

# Draw.
drawObject(ax, [[0,0,0], np.array(vec)/np.sqrt(np.dot(vec,vec))], color="gray")
drawObject(ax, [mObj, mObj+nVecObj], color="red")
drawObject(ax, obj, color="red")
drawObject(ax, [mRotObj, mRotObj + nVecRotObj], color="green")
drawObject(ax, rotObj, color="green")

# Plot cosmetics.
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')

# Check if the given vector and the normal of the rotated object are parallel (cross product should be zero).
print np.round(np.sum(np.cross(vec, nVecRotObj)**2), 5)