旋转正方形为标准

Question

Win7，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')

这将给出以下输出。。

绿色方块是xz平面上的原始方块，蓝色方块是变换后的方块，而给定的向量是红色的。在

正如你所看到的那样。经过许多小时的类似问题和回答，我仍然不明白为什么这不起作用。在

那么我在这里遗漏了什么？在

编辑：在把eldude在下面的评论中给出的Euler Angles link倒过来之后，我尝试了以下方法。。。。在

用基向量Ux、Uy和Uz定义了静态参考坐标系xyz平面中的正方形

使用方向向量'dirVec'作为我要旋转正方形的平面的法线。在

我决定使用x-约定和ZXZ旋转矩阵，如欧拉角链接中所述。在

我所采取的步骤

1）创建一个以Tx，Ty&Tz为基向量的旋转帧

^{pr2}$

2）通过取Uz&Tz的叉积，定义了一条节点线，一个沿平面UxUy&TxTy相交的向量

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')

Answer 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)