source: inundation/fit_interpolate/interpolate.py @ 2263

"""Least squares interpolation.

   Implements a least-squares interpolation.

   Ole Nielsen, Stephen Roberts, Duncan Gray, Christopher Zoppou
   Geoscience Australia, 2004.

DESIGN ISSUES
* what variables should be global?
- if there are no global vars functions can be moved around alot easier

* What will be the public interface to this class?

TO DO
* remove points outside the mesh ?(in interpolate_block)?
* geo-ref (in interpolate_block)
* add functional interpolate interface - in mesh and points, out interp data 
"""

import time

from Numeric import zeros, array, Float, Int, dot, transpose, concatenate, \
     ArrayType, allclose, take

from pyvolution.mesh import Mesh
from pyvolution.sparse import Sparse, Sparse_CSR
from pyvolution.cg_solve import conjugate_gradient, VectorShapeError
from coordinate_transforms.geo_reference import Geo_reference
from pyvolution.quad import build_quadtree
from utilities.numerical_tools import ensure_numeric
from utilities.polygon import inside_polygon

from search_functions import search_tree_of_vertices

class Interpolate:
    
    def __init__(self,
                 vertex_coordinates,
                 triangles,
                 mesh_origin = None,
                 verbose=False,
                 max_vertices_per_cell=30):


        """ Build interpolation matrix mapping from
        function values at vertices to function values at data points

        Inputs:

          vertex_coordinates: List of coordinate pairs [xi, eta] of
              points constituting a mesh (or an m x 2 Numeric array)
              Points may appear multiple times
              (e.g. if vertices have discontinuities)

          triangles: List of 3-tuples (or a Numeric array) of
              integers representing indices of all vertices in the mesh.

          mesh_origin: 3-tuples consisting of
              UTM zone, easting and northing.
              If specified vertex coordinates are assumed to be
              relative to their respective origins.

          max_vertices_per_cell: Number of vertices in a quad tree cell
          at which the cell is split into 4.

        """
        
        # Initialise variabels
        self._A_can_be_reused = False
        self._point_coordinates = None
        
        #Convert input to Numeric arrays
        triangles = ensure_numeric(triangles, Int)
        vertex_coordinates = ensure_numeric(vertex_coordinates, Float)

        #Build underlying mesh
        if verbose: print 'Building mesh'
        #self.mesh = General_mesh(vertex_coordinates, triangles,
        #FIXME: Trying the normal mesh while testing precrop,
        #       The functionality of boundary_polygon is needed for that

        #FIXME - geo ref does not have to go into mesh.
        # Change the point co-ords to conform to the
        # mesh co-ords early in the code

        #FIXME: geo_ref can also be a geo_ref object
        #FIXME: move this to interpolate_block
        if mesh_origin is None:
            geo = None
        else:
            geo = Geo_reference(mesh_origin[0],mesh_origin[1],mesh_origin[2])
        self.mesh = Mesh(vertex_coordinates, triangles,
                         geo_reference = geo)
        
        self.mesh.check_integrity()

        self.root = build_quadtree(self.mesh,
                              max_points_per_cell = max_vertices_per_cell)
        
        
    def _build_interpolation_matrix_A(self,
                                     point_coordinates,
                                     verbose = False):
        """Build n x m interpolation matrix, where
        n is the number of data points and
        m is the number of basis functions phi_k (one per vertex)

        This algorithm uses a quad tree data structure for fast binning
        of data points
        origin is a 3-tuple consisting of UTM zone, easting and northing.
        If specified coordinates are assumed to be relative to this origin.

        This one will override any data_origin that may be specified in
        instance interpolation

        Preconditions
        Point_coordindates and mesh vertices have the same origin.
        """


        
        #Convert point_coordinates to Numeric arrays, in case it was a list.
        point_coordinates = ensure_numeric(point_coordinates, Float)
        
        #Remove points falling outside mesh boundary
        # do this bit later - that sorta means this becomes an object
        # get a list of what indices are outside the boundary
        # maybe fill these rows with n/a?

        
        #Build n x m interpolation matrix
        m = self.mesh.coordinates.shape[0] #Nbr of basis functions (1/vertex)
        n = point_coordinates.shape[0]     #Nbr of data points

        if verbose: print 'Number of datapoints: %d' %n
        if verbose: print 'Number of basis functions: %d' %m

        A = Sparse(n,m)

        #Compute matrix elements
        for i in range(n):
            #For each data_coordinate point
            if verbose and i%((n+10)/10)==0: print 'Doing %d of %d' %(i, n)
            x = point_coordinates[i]
            element_found, sigma0, sigma1, sigma2, k = \
                           search_tree_of_vertices(self.root, self.mesh, x)
            #Update interpolation matrix A if necessary
            if element_found is True:
                #Assign values to matrix A

                j0 = self.mesh.triangles[k,0] #Global vertex id for sigma0
                j1 = self.mesh.triangles[k,1] #Global vertex id for sigma1
                j2 = self.mesh.triangles[k,2] #Global vertex id for sigma2

                sigmas = {j0:sigma0, j1:sigma1, j2:sigma2}
                js     = [j0,j1,j2]

                for j in js:
                    A[i,j] = sigmas[j]
            else:
                print 'Could not find triangle for point', x 
        return A

    def _search_tree_of_vertices_OBSOLETE(self, root, mesh, x):
        """
        Find the triangle (element) that the point x is in.

        root: A quad tree of the vertices
        Return the associated sigma and k values
           (and if the element was found) .
        """
        #Find triangle containing x:
        element_found = False

        # This will be returned if element_found = False
        sigma2 = -10.0
        sigma0 = -10.0
        sigma1 = -10.0
        k = -10.0
            
        #Find vertices near x
        candidate_vertices = root.search(x[0], x[1])
        is_more_elements = True

        element_found, sigma0, sigma1, sigma2, k = \
                       self._search_triangles_of_vertices(mesh,
                                                          candidate_vertices, x)
        while not element_found and is_more_elements:
            candidate_vertices, branch = root.expand_search()
            if branch == []:
                # Searching all the verts from the root cell that haven't
                # been searched.  This is the last try
                element_found, sigma0, sigma1, sigma2, k = \
                      self._search_triangles_of_vertices(mesh,
                                                         candidate_vertices, x)
                is_more_elements = False
            else:
                element_found, sigma0, sigma1, sigma2, k = \
                      self._search_triangles_of_vertices(mesh,
                                                         candidate_vertices, x)

        return element_found, sigma0, sigma1, sigma2, k

    def _search_triangles_of_vertices_OBSOLETE(self, mesh, candidate_vertices, x):
        #Find triangle containing x:
        element_found = False

        # This will be returned if element_found = False
        sigma2 = -10.0
        sigma0 = -10.0
        sigma1 = -10.0
        k = -10.0
        #print "*$* candidate_vertices", candidate_vertices
        #For all vertices in same cell as point x
        for v in candidate_vertices:
            #FIXME (DSG-DSG): this catches verts with no triangle.
            #Currently pmesh is producing these.
            #this should be stopped, 
            if mesh.vertexlist[v] is None:
                continue
            #for each triangle id (k) which has v as a vertex
            for k, _ in mesh.vertexlist[v]:
                #Get the three vertex_points of candidate triangle
                xi0 = mesh.get_vertex_coordinate(k, 0)
                xi1 = mesh.get_vertex_coordinate(k, 1)
                xi2 = mesh.get_vertex_coordinate(k, 2)

                #Get the three normals
                n0 = mesh.get_normal(k, 0)
                n1 = mesh.get_normal(k, 1)
                n2 = mesh.get_normal(k, 2)

                #Compute interpolation
                sigma2 = dot((x-xi0), n2)/dot((xi2-xi0), n2)
                sigma0 = dot((x-xi1), n0)/dot((xi0-xi1), n0)
                sigma1 = dot((x-xi2), n1)/dot((xi1-xi2), n1)
                    
                #FIXME: Maybe move out to test or something
                epsilon = 1.0e-6
                assert abs(sigma0 + sigma1 + sigma2 - 1.0) < epsilon

                #Check that this triangle contains the data point
                
                #Sigmas can get negative within
                #machine precision on some machines (e.g nautilus)
                #Hence the small eps
                eps = 1.0e-15
                if sigma0 >= -eps and sigma1 >= -eps and sigma2 >= -eps:
                    element_found = True
                    break

            if element_found is True:
                #Don't look for any other triangle
                break
        return element_found, sigma0, sigma1, sigma2, k



     # FIXME: What is a good start_blocking_count value?
    def interpolate(self, f, point_coordinates = None,
                    start_blocking_len = 500000, verbose=False):
        """Interpolate mesh data f to determine values, z, at points.

        f is the data on the mesh vertices.

        The mesh values representing a smooth surface are
        assumed to be specified in f.

        Inputs:
          f: Vector or array of data at the mesh vertices.
              If f is an array, interpolation will be done for each column as
              per underlying matrix-matrix multiplication
          point_coordinates: Interpolate mesh data to these positions.
              List of coordinate pairs [x, y] of
              data points (or an nx2 Numeric array)
              If point_coordinates is absent, the points inputted last time
              this method was called are used, if possible.
          start_blocking_len: If the # of points is more or greater than this,
              start blocking 

        Output:
          Interpolated values at inputted points (z).
        """
        
        # Can I interpolate, based on previous point_coordinates? 
        if point_coordinates is None:
            if self._A_can_be_reused is True and \
            len(self._point_coordinates) < start_blocking_len: 
                z = self._get_point_data_z(f,
                                           verbose=verbose)
            elif self._point_coordinates is not None:
                #     if verbose, give warning
                if verbose:
                    print 'WARNING: Recalculating A matrix, due to blocking.'
                point_coordinates = self._point_coordinates
            else:
                #There are no good point_coordinates. import sys; sys.exit()
                msg = 'ERROR (interpolate.py): No point_coordinates inputted'
                raise msg
            
        if point_coordinates is not None:
            self._point_coordinates = point_coordinates
            if len(point_coordinates) < start_blocking_len or \
                   start_blocking_len == 0:
                self._A_can_be_reused = True
                z = self.interpolate_block(f, point_coordinates,
                                           verbose=verbose)
            else:
                #Handle blocking
                self._A_can_be_reused = False
                start=0
                z = self.interpolate_block(f, point_coordinates[0:0])
                for end in range(start_blocking_len
                                 ,len(point_coordinates)
                                 ,start_blocking_len):
                    t = self.interpolate_block(f, point_coordinates[start:end],
                                               verbose=verbose)
                    z = concatenate((z,t))
                    start = end
                end = len(point_coordinates)
                t = self.interpolate_block(f, point_coordinates[start:end],
                                           verbose=verbose)
                z = concatenate((z,t))
        return z

    def interpolate_block(self, f, point_coordinates = None, verbose=False):
        """
        Call this if you want to control the blocking or make sure blocking
        doesn't occur.

        See interpolate for doc info.
        """
        if point_coordinates is not None:
            self._A =self._build_interpolation_matrix_A(point_coordinates,
                                                       verbose=verbose)
        return self._get_point_data_z(f)

    def _get_point_data_z(self, f, verbose=False):
        return self._A * f
        
#-------------------------------------------------------------
if __name__ == "__main__":
    a = [0.0, 0.0]
    b = [0.0, 2.0]
    c = [2.0,0.0]
    points = [a, b, c]
    vertices = [ [1,0,2] ]   #bac
    
    data = [ [2.0/3, 2.0/3] ] #Use centroid as one data point
    
    interp = Interpolate(points, vertices) #, data)
    A  = interp._build_interpolation_matrix_A(data, verbose=True)
    A = A.todense()
    print "A",A
    assert allclose(A, [[1./3, 1./3, 1./3]])
    print "finished"