Rotate square to be normal to a vector

Question

Win 7, x64, Python 2.7

I'm trying to rotate a square that is initially in the xz plane so that its normal aligns with a given 3D vector. Also I am translating the square to the start of the vector but that isnt a problem.

The path I have taken is as follows,

1) Find the axis of rotation via the cross product of the given vector & the square's normal, a unit vector in the y direction in this case.

2) Find the angle of rotation via the dot product of the given vector and the square's normal.

3) Build appropriate rotation matrix.

4) Apply rotation matrix to the vertices of the square.

5) Translate to the start of the given vector.

The code..

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

This gives the following output..

The green square is the original in the xz plane, the blue square the transformed square & the given vector is in red.

As you can see its well off. After many hours pouring through similar questions & replies, I am still none the wiser as to why this does not work.

So what am I missing here?

EDIT: After pouring over the Euler Angles link, given by El Dude in the comments below, I tried the following....

Defined the square in yz plane of a static frame of reference xyz with basis vectors Ux, Uy & Uz

Used a direction vector 'dirVec' as the normal for the plane I want to rotate my square into.

I decided to use the x-convention and the ZXZ rotation matrix as discribed in Euler angles link.

Steps I have taken,

1) Create a rotated frame with Tx, Ty & Tz as basis vectors;

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

2) Defined a Node Line, a vector along the intersection of the planes UxUy & TxTy by taking the cross product of Uz & Tz

3) Defined the Euler angles as per the definitions in the above link

4) Defined the ZXZ rotation matrix as per the above link

5) Applied rotation matrix to coordinates of square's vertices

It doesn't work, something odd is happening, no matter what the value of 'dirVec' alpha always comes out as 0.

Is there something obvious going on that I'm just missing?

Here's the amended code...

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

Here's a solution that I came up with - it should work, although there wasn't a whole lot of testing. The solution is somewhat more general, as it would work for any 2D object of any orientation, the only thing you have to adjust are the vertices stored in obj (this could be done better but here I just created a list of points by hand).

Note, that I defined mObj as the "center" of the object - this does not change the functionality but is the anchor point of the normal vector that is displayed.

Here's some explanation for the math: What we need to do is to find the right rotation axis and angle, such that we only need one matrix multiplication (in principle you could use the Euler angles which would be an equivalent solution). The angle is easy, since it is given by the dot-product:

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

where theta is the angle between the vector a and b. To find the rotation axis, we can use the normal vector of the plane spanned by a and b, i.e. use the cross product and normalize it:

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

Note that this vector is orthogonal to a and b, hence the axis we are looking for.

Hope this helps.

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)