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)

def rotation_angle(helix_vector, axis_vector, rotation_vector):
    reference_vector = np.cross(np.cross(helix_vector, axis_vector), helix_vector)
    second_reference_vector = np.cross(axis_vector, helix_vector)
    screw_angle = mdamath.angle(reference_vector, rotation_vector)
    alt_screw_angle = mdamath.angle(second_reference_vector, rotation_vector)
    updown = np.cross(reference_vector, rotation_vector)

    if not (np.pi < screw_angle < 3 * np.pi / 4):
        if screw_angle < np.pi / 4 and alt_screw_angle < np.pi / 2:
            screw_angle = np.pi / 2 - alt_screw_angle
        elif screw_angle < np.pi / 4 and alt_screw_angle > np.pi / 2:
            screw_angle = alt_screw_angle - np.pi / 2
        elif screw_angle > 3 * np.pi / 4 and alt_screw_angle < np.pi / 2:
            screw_angle = np.pi / 2 + alt_screw_angle
        elif screw_angle > 3 * np.pi / 4 and alt_screw_angle > np.pi / 2:
            screw_angle = 3 * np.pi / 2 - alt_screw_angle
        else:
            logger.debug("Big Screw Up: screw_angle=%g degrees", np.rad2deg(screw_angle))

    if mdamath.norm(updown) == 0:
        logger.warn("PROBLEM (vector is at 0 or 180)")

    helix_dot_rehelix = mdamath.angle(updown, helix_vector)

    #if ( helix_dot_rehelix < np.pi/2 and helix_dot_rehelix >= 0 )or helix_dot_rehelix <-np.pi/2:
    if (-np.pi / 2 < helix_dot_rehelix < np.pi / 2) or (helix_dot_rehelix > 3 * np.pi / 2):
        screw_angle = -screw_angle

    return screw_angle

def TEME_to_ITRF(jd_ut1, rTEME, vTEME, xp=0.0, yp=0.0):
    """Convert TEME position and velocity into standard ITRS coordinates.

    This converts a position and velocity vector in the idiosyncratic
    True Equator Mean Equinox (TEME) frame of reference used by the SGP4
    theory into vectors into the more standard ITRS frame of reference.
    The velocity should be provided in units per day, not per second.

    From AIAA 2006-6753 Appendix C.

    """
    theta, theta_dot = theta_GMST1982(jd_ut1)
    zero = theta_dot * 0.0
    angular_velocity = array([zero, zero, -theta_dot])
    R = rot_z(-theta)

    if len(rTEME.shape) == 1:
        rPEF = (R).dot(rTEME)
        vPEF = (R).dot(vTEME) + cross(angular_velocity, rPEF)
    else:
        rPEF = einsum('ij...,j...->i...', R, rTEME)
        vPEF = einsum('ij...,j...->i...', R, vTEME) + cross(
            angular_velocity, rPEF, 0, 0).T

    if xp == 0.0 and yp == 0.0:
        rITRF = rPEF
        vITRF = vPEF
    else:
        W = (rot_x(yp)).dot(rot_y(xp))
        rITRF = (W).dot(rPEF)
        vITRF = (W).dot(vPEF)
    return rITRF, vITRF

def dihedral(vec1,vec2,vec3,vec4):
    """
    Returns a float value for the dihedral angle between 
    the four vectors. They define the bond for which the 
    torsion is calculated (~) as:
    V1 - V2 ~ V3 - V4 

    The vectors vec1 .. vec4 can be array objects, lists or tuples of length 
    three containing floats. 
    For Scientific.geometry.Vector objects the behavior is different 
    on Windows and Linux. Therefore, the latter is not a featured input type 
    even though it may work.
    
    If the dihedral angle cant be calculated (because vectors are collinear),
    the function raises a DihedralGeometryError
    """    
    # create array instances.
    v1,v2,v3,v4 =create_vectors(vec1,vec2,vec3,vec4)
    all_vecs = [v1,v2,v3,v4]

    # rule out that two of the atoms are identical
    # except the first and last, which may be.
    for i in range(len(all_vecs)-1):
        for j in range(i+1,len(all_vecs)):
            if i>0 or j<3: # exclude the (1,4) pair
                equals = all_vecs[i]==all_vecs[j]
                if equals.all():
                    raise DihedralGeometryError(\
                        "Vectors #%i and #%i may not be identical!"%(i,j))

    # calculate vectors representing bonds
    v12 = v2-v1
    v23 = v3-v2
    v34 = v4-v3

    # calculate vectors perpendicular to the bonds
    normal1 = cross(v12,v23)
    normal2 = cross(v23,v34)

    # check for linearity
    if norm(normal1) == 0 or norm(normal2)== 0:
        raise DihedralGeometryError(\
            "Vectors are in one line; cannot calculate normals!")

    # normalize them to length 1.0
    normal1 = normal1/norm(normal1)
    normal2 = normal2/norm(normal2)

    # calculate torsion and convert to degrees
    torsion = angle(normal1,normal2) * 180.0/pi

    # take into account the determinant
    # (the determinant is a scalar value distinguishing
    # between clockwise and counter-clockwise torsion.
    if scalar(normal1,v34) >= 0:
        return torsion
    else:
        torsion = 360-torsion
        if torsion == 360: torsion = 0.0
        return torsion

 def getCurvePoints(self,P0,P1,P2,R,n=20):
     """Generates a curve. P0,P1 and P2 define a corner and R defines the radius of the
     tangential circle element. n gives the number of point to approximate the curve.
     P1 is the corner itself and P0 and P2 give the tangents.
     """
     P0=np.array(P0)
     P1=np.array(P1)
     P2=np.array(P2)
     o1=(P0-P1)/np.sqrt(np.dot(P0-P1,P0-P1))
     o2=(P2-P1)/np.sqrt(np.dot(P2-P1,P2-P1))
     if np.arcsin(np.cross(o1,o2)) > 0:
         a=1.
         b=-1.
     else:
         a=-1.
         b=1.
     
     v1=R*np.dot(np.array([[0.,b],[a,0.]]),o1)
     v2=R*np.dot(np.array([[0.,a],[b,0.]]),o2)
     dv=v2-v1
     a=np.array([[o1[0],-o2[0]],[o1[1],-o2[1]]])
     b=dv
     x=np.linalg.solve(a,b)
     circleCenter= P1+x[0]*o1+v1
     angle = np.arcsin(np.cross(v2/R,v1/R))
     points=[]
     for i in range(n+1):
         x=-i*angle/n
         rot = np.array([[np.cos(x),-np.sin(x)],
                          [np.sin(x),np.cos(x)]])
         points.append(circleCenter+np.dot(rot,-v1))
     return points

    def __init__(self, position, direction, N=720, M=486,
                 scale=1,
                 orthographic=False):
        super(Camera, self).__init__(position)

        self._orth = orthographic

        self.z_p = 1 #focal length
        self.up = Vector(0, 1, 0)
        self.direction = direction

        #centre of the image screen
        self.c = self.position + self.z_p * self.direction 

        #horizontal screen direction
        self.u_x = (np.cross(self.direction, self.up) /
                   norm(np.cross(self.direction, self.up)))
        #vertical screen direction
        self.u_z = - self.direction
        #normal to the viewscreen
        self.u_y = np.cross(self.u_z, self.u_x)

        self.m = int(M * scale)
        self.n = int(N * scale)

        factor = 0.5
        self.hight = (1)*factor
        self.width = (16/9.0)*factor

        #vertical distance between pixels
        self.dydi = self.hight / float(self.m)
        #horizontal distance between pixels
        self.dxdi = self.width / float(self.n)