Geek Answers Handbook

Turn the square to be normal to the vector

Win 7, x64, Python 2.7

I am trying to rotate a square that is initially in the xz plane, so that its normal aligns with this three-dimensional vector. I am also putting the square at the beginning of the vector, but that is not a problem.

The path I made is as follows:

1) Find the axis of rotation through the transverse product of the given vector and the square normal, the unit vector in the y direction in this case.

2) Find the angle of rotation through the point product of this vector and the square normal.

3) Create the appropriate rotation matrix.

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

5) Translate to the beginning of this vector.

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 result. enter image description here

xz, - , - .

. , , , .

, ?

: Euler Angles, El Dude , ....

yz xyz Ux, Uy Uz

'dirVec' , .

- ZXZ, .

, ,

1) Tx, Ty Tz ;

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

2) a Node , UxUy TxTy, Uz 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')
+6
1

, - , . , 2D- , , , - , obj ( , ).

, mObj "" -. , ,

: , ( , ). , :

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

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)
+1

More articles:


All Articles