Python numpy.cross函数代码示例

 def get_orientation_from_lines(self, v0, v1):
     """ 
     Takes in two vectors, v0 and v1, (which are KNOWN to be in the same plane) and finds 
     the normal, and creates a rotation matrix from v0, v1, and the normal; then converts this 
     rotation matrix to a quaternion 
     """
     v0, v1 = np.array(v0), np.array(v1)
     v0 = v0 / np.linalg.norm(v0) 
     v1 = v1 / np.linalg.norm(v1) 
     n = np.cross(v0, v1)
     parallel = average_vectors(v0, v1)
     parallel = parallel / np.linalg.norm(parallel)
     third = np.cross(n, parallel)
     third = third / np.linalg.norm(third)
     #n = n / np.linalg.norm(n)
     #v1 = np.cross(n, v0)
     #v1 = v1 / np.linalg.norm(v1)
     #rotMat = np.vstack((n, v1, v0)).T
     rotMat = np.vstack((parallel, third, n)).T
     matrix = rotMat
     tbRot = tfx.tb_angles(matrix).matrix        
     #tbRot = self.rotate(-90, "yaw", tbRot)    #FIXME: get correct yaw pitch roll values
     #tbRot = self.rotate(60, "pitch", tbRot)
     tbRot = self.rotate(180, "roll", tbRot)
     quat = tfx.tb_angles(tbRot).quaternion
     return list(quat)

def rotation_matrix(a1, a2, b1, b2):
    """Returns a rotation matrix that rotates the vectors *a1* in the
    direction of *a2* and *b1* in the direction of *b2*.

    In the case that the angle between *a2* and *b2* is not the same
    as between *a1* and *b1*, a proper rotation matrix will anyway be
    constructed by first rotate *b2* in the *b1*, *b2* plane.
    """
    a1 = np.asarray(a1, dtype=float) / np.linalg.norm(a1)
    b1 = np.asarray(b1, dtype=float) / np.linalg.norm(b1)
    c1 = np.cross(a1, b1)
    c1 /= np.linalg.norm(c1)      # clean out rounding errors...

    a2 = np.asarray(a2, dtype=float) / np.linalg.norm(a2)
    b2 = np.asarray(b2, dtype=float) / np.linalg.norm(b2)
    c2 = np.cross(a2, b2)
    c2 /= np.linalg.norm(c2)      # clean out rounding errors...

    # Calculate rotated *b2*
    theta = np.arccos(np.dot(a2, b2)) - np.arccos(np.dot(a1, b1))
    b3 = np.sin(theta) * a2 + np.cos(theta) * b2
    b3 /= np.linalg.norm(b3)      # clean out rounding errors...

    A1 = np.array([a1, b1, c1])
    A2 = np.array([a2, b3, c2])
    R = np.linalg.solve(A1, A2).T
    return R

def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
    ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
        and the equations of a table its resting on, and the general direction of the robot
        Fit a barrell drill
     '''

    if not drillPoints.GetNumberOfPoints():
        return

    vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
    drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])

    if not drillBarrelPoints.GetNumberOfPoints():
        return


    # fit line to drill barrel points
    linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)

    if np.dot(lineDirection, forwardDirection) < 0:
        lineDirection = -lineDirection

    vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)


    pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')

    dists = np.dot(pts-linePoint, lineDirection)

    p1 = linePoint + lineDirection*np.min(dists)
    p2 = linePoint + lineDirection*np.max(dists)

    p1 = projectPointToPlane(p1, plane_origin, plane_normal)
    p2 = projectPointToPlane(p2, plane_origin, plane_normal)


    d = DebugData()
    d.addSphere(p1, radius=0.01)
    d.addSphere(p2, radius=0.01)
    d.addLine(p1, p2)
    vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)


    drillToBasePoint = np.array([-0.07,  0.0  , -0.12])

    zaxis = plane_normal
    xaxis = lineDirection
    xaxis /= np.linalg.norm(xaxis)
    yaxis = np.cross(zaxis, xaxis)
    yaxis /= np.linalg.norm(yaxis)
    xaxis = np.cross(yaxis, zaxis)
    xaxis /= np.linalg.norm(xaxis)

    t = getTransformFromAxes(xaxis, yaxis, zaxis)
    t.PreMultiply()
    t.Translate(-drillToBasePoint)
    t.PostMultiply()
    t.Translate(p1)

    return t

def build(lattice, basis, layers, tol):
    surf = lattice.copy()
    scaled = solve(basis.T, surf.get_scaled_positions().T).T
    scaled -= np.floor(scaled + tol)
    surf.set_scaled_positions(scaled)
    surf.set_cell(np.dot(basis, surf.cell), scale_atoms=True)
    surf *= (1, 1, layers)

    a1, a2, a3 = surf.cell
    surf.set_cell([a1, a2,
                   np.cross(a1, a2) * np.dot(a3, np.cross(a1, a2)) /
                   norm(np.cross(a1, a2))**2])
    
    # Change unit cell to have the x-axis parallel with a surface vector
    # and z perpendicular to the surface:
    a1, a2, a3 = surf.cell
    surf.set_cell([(norm(a1), 0, 0),
                   (np.dot(a1, a2) / norm(a1),
                    np.sqrt(norm(a2)**2 - (np.dot(a1, a2) / norm(a1))**2), 0),
                   (0, 0, norm(a3))],
                  scale_atoms=True)

    surf.pbc = (True, True, False)

    # Move atoms into the unit cell:
    scaled = surf.get_scaled_positions()
    scaled[:, :2] %= 1
    surf.set_scaled_positions(scaled)

    return surf

def SegIntersect(A1, A2, B1, B2):
    """The function returns the intersection or the points of closest approach if lines are skewed.
    If lines are parallel, NaN is returned.
    INPUT:
        A1  -float(3,n), [x,y,z;nsegments] cordinates of 1st point(s) of 1st segment(s)
        A2  -float(3,n), [x,y,z;nsegments] cordinates of 2nd point(s) of 1st segment(s)
        B1  -float(3,n), [x,y,z;nsegments] cordinates of 1st point(s) of 2nd segment(s)
        B2  -float(3,n), [x,y,z;nsegments] cordinates of 2nd point(s) of 2nd segment(s)
    OUTPUT:
        A0  -float(3,n), [x,y,z;nsegments] coordinates of intersection point (=B0) or closet point to 2nd line on 1st segment,
        B0  -float(3,n), [x,y,z;nsegments] coordinates of intersection point (=A0) or closet point to 1st line on 2nd segment,
        OR  -NaN
    """

    # reshape A1..B2 in case they have 1 dimension only
    A1 = A1.reshape(3, -1)
    A2 = A2.reshape(3, -1)
    B1 = B1.reshape(3, -1)
    B2 = B2.reshape(3, -1)

    vec = np.cross(A2 - A1, B2 - B1, 0, 0, 0)
    nA = np.sum(np.cross(B2 - B1, A1 - B1, 0, 0, 0) * vec, axis=0) * np.ones(A1.shape[1])
    nB = np.sum(np.cross(A2 - A1, A1 - B1, 0, 0, 0) * vec, axis=0) * np.ones(A1.shape[1])
    d = np.sum(vec ** 2, axis=0) * np.ones(A1.shape[1])

    A0 = np.ones(A1.shape) * np.NaN
    B0 = A0.copy()
    idx = np.nonzero(d)[0]
    A0[:, idx] = A1[:, idx] + (nA[idx] / d[idx]) * (A2[:, idx] - A1[:, idx])
    B0[:, idx] = B1[:, idx] + (nB[idx] / d[idx]) * (B2[:, idx] - B1[:, idx])

    return A0, B0

def my_solid_angle(center, coords):
    """
    Helper method to calculate the solid angle of a set of coords from the
    center.

    Args:
        center:
            Center to measure solid angle from.
        coords:
            List of coords to determine solid angle.

    Returns:
        The solid angle.
    """
    o = np.array(center)
    r = [np.array(c) - o for c in coords]
    r.append(r[0])
    n = [np.cross(r[i + 1], r[i]) for i in range(len(r) - 1)]
    n.append(np.cross(r[1], r[0]))
    phi = 0.0
    for i in range(len(n) - 1):
        try:
            value = math.acos(-np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1])))
        except ValueError:
            mycos = -np.dot(n[i], n[i + 1]) / (np.linalg.norm(n[i]) * np.linalg.norm(n[i + 1]))
            if 0.999999999999 < mycos < 1.000000000001:
                value = math.acos(1.0)
            elif -0.999999999999 > mycos > -1.000000000001:
                value = math.acos(-1.0)
            else:
                raise SolidAngleError(mycos)
        phi += value
    return phi + (3 - len(r)) * math.pi

def lookAt(eye, center, up):
    ret = numpy.eye(4, dtype=numpy.float32)

    Z = numpy.array(eye, numpy.float32) - numpy.array(center, numpy.float32)
    Z = normalize(Z)
    Y = numpy.array(up, numpy.float32)
    X = numpy.cross(Y, Z)
    Y = numpy.cross(Z, X)

    X = normalize(X)
    Y = normalize(Y)

    ret[0][0] = X[0]
    ret[1][0] = X[1]
    ret[2][0] = X[2]
    ret[3][0] = -numpy.dot(X, eye)
    ret[0][1] = Y[0]
    ret[1][1] = Y[1]
    ret[2][1] = Y[2]
    ret[3][1] = -numpy.dot(Y, eye)
    ret[0][2] = Z[0]
    ret[1][2] = Z[1]
    ret[2][2] = Z[2]
    ret[3][2] = -numpy.dot(Z, eye)
    ret[0][3] = 0
    ret[1][3] = 0
    ret[2][3] = 0
    ret[3][3] = 1.0
    return ret

def get_neuromag_transform(lpa, rpa, nasion):
    """Creates a transformation matrix from RAS to Neuromag-like space

    Resets the origin to mid-distance of peri-auricular points with nasion
    passing through y-axis.
    (mne manual, pg. 97)

    Parameters
    ----------
    lpa : numpy.array, shape = (1, 3)
        Left peri-auricular point coordinate.
    rpa : numpy.array, shape = (1, 3)
        Right peri-auricular point coordinate.
    nasion : numpy.array, shape = (1, 3)
        Nasion point coordinate.

    Returns
    -------
    trans : numpy.array, shape = (3, 3)
        Transformation matrix to Neuromag-like space.
    """
    origin = (lpa + rpa) / 2
    nasion = nasion - origin
    lpa = lpa - origin
    rpa = rpa - origin
    axes = np.empty((3, 3))
    axes[1] = nasion / linalg.norm(nasion)
    axes[2] = np.cross(axes[1], lpa - rpa)
    axes[2] /= linalg.norm(axes[2])
    axes[0] = np.cross(axes[1], axes[2])

    trans = linalg.inv(axes)
    return trans

def view_transformation(E, R, V):
    n = (E - R)
    ## new
#    n /= mod(n)
#    u = np.cross(V,n)
#    u /= mod(u)
#    v = np.cross(n,u)
#    Mr = np.diag([1.]*4)
#    Mt = np.diag([1.]*4)
#    Mr[:3,:3] = u,v,n
#    Mt[:3,-1] = -E
    ## end new

    ## old
    n = n / mod(n)
    u = np.cross(V, n)
    u = u / mod(u)
    v = np.cross(n, u)
    Mr = [[u[0],u[1],u[2],0],
          [v[0],v[1],v[2],0],
          [n[0],n[1],n[2],0],
          [0,   0,   0,   1],
          ]
    #
    Mt = [[1, 0, 0, -E[0]],
          [0, 1, 0, -E[1]],
          [0, 0, 1, -E[2]],
          [0, 0, 0, 1]]
    ## end old

    return np.dot(Mr, Mt)

def _dihedral(xyz, indices, out=None):
    """Compute the dihedral angles of traj for the atom indices in indices.

    Parameters
    ----------
    xyz : np.ndarray, shape=(num_frames, num_atoms, 3), dtype=float
        The XYZ coordinates of a trajectory
    indices : np.ndarray, shape=(num_dihedrals, 4), dtype=int
        Atom indices to compute dihedrals.

    Returns
    -------
    dih : np.ndarray, shape=(num_dihedrals), dtype=float
        dih[i,j] gives the dihedral angle at traj[i] correponding to indices[j].

    """
    x0 = xyz[:, indices[:, 0]]
    x1 = xyz[:, indices[:, 1]]
    x2 = xyz[:, indices[:, 2]]
    x3 = xyz[:, indices[:, 3]]

    b1 = x1 - x0
    b2 = x2 - x1
    b3 = x3 - x2

    c1 = np.cross(b2, b3)
    c2 = np.cross(b1, b2)

    p1 = (b1 * c1).sum(-1)
    p1 *= (b2 * b2).sum(-1) ** 0.5
    p2 = (c1 * c2).sum(-1)

    return np.arctan2(p1, p2, out)

 def _facenorm_cross_edge(self):
     ppts = self.ppts
     fnorms = self.face_normals
     fe12 = np.cross(fnorms, ppts[:,1] - ppts[:,0])
     fe23 = np.cross(fnorms, ppts[:,2] - ppts[:,1])
     fe31 = np.cross(fnorms, ppts[:,0] - ppts[:,2])
     return fe12, fe23, fe31

    def trimField(self):
        trimStarts = self.trimOutline.starts
        trimVectors = self.trimOutline.vectors
        fieldStarts = self.design.starts
        fieldVectors = self.design.vectors
        trimLen = len(self.trimOutline)
        fieldLen = len(self.design)
        Q_Less_P = fieldStarts - trimStarts.reshape(trimLen,1,2)
        denom = np.cross(trimVectors, fieldVectors.reshape(fieldLen,1,2))
        all_t = np.cross(Q_Less_P, trimVectors.reshape(trimLen,1,2)).transpose()/denom
        all_u = np.cross(Q_Less_P, fieldVectors).transpose()/denom
        
        for t, u, line in zip(all_t, all_u, self.design):
            if not self.trimOutline.lineOutsideBoundingBox(line):
                pointSet = set([line.start])
                t = t[(0 <= u) & (u <= 1) & (0 <= t) & (t <= 1)]

                pointSet |= set(Point(line.start.x + line.vector[c.X]*value,
                                    line.start.y+line.vector[c.Y]*value)
                                    for value in t)

                pointSet.add(line.end)
                pointList = sorted(list(pointSet))
                pointVectors = np.array([point.normalVector for point in pointList])
                
                """ Calculation for midPoints from here:
                http://stackoverflow.com/questions/23855976/middle-point-of-each-pair-of-an-numpy-array
                """
                midPoints = (pointVectors[1:] + pointVectors[:-1])/2.0
                for i in range(len(midPoints)):
                    if self.trimOutline.isInside(midPoints[i]):
                        self.append(Line(pointList[i], pointList[i+1]))

	def add(self, args):
		dL, nL, stdL, dR, nR, stdR = args

		rL = np.cross(np.array([0,0,1]), nL)
		sL = np.cross(rL, nL)
		RL = np.array([rL, sL, nL])

		rR = np.cross(np.array([0,0,1]), nR)
		sR = np.cross(rR, nR)
		RR = np.array([rR, sR, nR])

		self.addPlane(RL, dL*nL)
		self.addPlane(RR, dR*nR)

		self.ax.plot([0,50], [0,0], [0,0], linewidth=2.0, color='red')
		self.ax.plot([0,0], [0,0], [0,50], linewidth=2.0, color='green')
		self.ax.plot([0,0], [0,50], [0,0], linewidth=2.0, color='blue')

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

		self.ax.text(-100,0,0, str(round(stdL, 5)), fontsize=15)
		self.ax.text(100,0,0, str(round(stdR, 5)), fontsize=15)

		self.ax.set_xlim(-150, 150)
		self.ax.set_ylim(0, 400)
		self.ax.set_zlim(-150, 150)

		self.ax.invert_xaxis()
		self.ax.invert_yaxis()
		self.ax.invert_zaxis()

		self.canvas.draw()
		self.Layout()

def getSurfaceVectors(face, camera):

    a = np.array([face[0][0],face[1][0], face[2][0]]).T
    #a = np.array(np.dot(camera.P,a))[0]

    b = np.array([face[0][1],face[1][1], face[2][1]]).T
    #b = np.array(np.dot(camera.P,b))[0]

    c = np.array([face[0][2],face[1][2], face[2][2]]).T
    #c = np.array(np.dot(camera.P,c))[0]

    a = np.array([a[0], a[1],a[2]])
    b = np.array([b[0], b[1],b[2]])
    c = np.array([c[0], c[1],c[2]])

    #vector between two face points
    v_ba = np.array([a[0]-b[0],a[1]-b[1],a[2]-b[2]])
    v_bc = np.array([c[0]-b[0],c[1]-b[1],c[2]-b[2]])
    #surface center
    cent = ((v_ba+v_bc)/2) + b

    #normalize norm --- ghurt is working here.
    norm = np.cross(v_ba,v_bc)/np.linalg.norm(np.cross(v_ba,v_bc))

    #surface center and norm
    return (cent, norm)

    def __init__(self, p1, p2, p3):

        # initialize points
        self.p1 = np.array(p1)
        self.p2 = np.array(p2)
        self.p3 = np.array(p3)

        # set vectors form p1 to p2 and from p1 to p3
        self.v12 = self.p2 - self.p1
        self.v13 = self.p3 - self.p1

	self.v12 = self.v12 / np.linalg.norm(self.v12)
        # calculate the normal to v12 and v13
        self.n = np.cross(self.v12, self.v13)
        self.n = self.n / np.linalg.norm(self.n)

        # calculate corresponding axes of the plane, having p1 as base
        # axisX = v12 / |v12|
        self.axisX = self.v12 / np.linalg.norm(self.v12)
        self.axisY = np.cross(self.n, self.axisX)
        #self.axisY = self.axisY / np.linalg.norm(self.axisY)

        self.rot = np.matrix([np.array(self.axisX), np.array(self.axisY),
                              np.array(self.n)])

        self.q = [ 0,0,0,0 ]
        self.q[3] = np.sqrt(1 + self.rot[0,0] + self.rot[1,1] + self.rot[2,2]) \
                / 2
        self.q[0] = (self.rot[2,1] - self.rot[1,2]) / (4 * self.q[3])
        self.q[1] = (self.rot[0,2] - self.rot[2,0]) / (4 * self.q[3])
        self.q[2] = (self.rot[1,0] - self.rot[0,1]) / (4 * self.q[3])

	self.scale = np.linalg.norm(self.p2 - self.p1)