# td_dda Three-Dimensional Digital Differential Analyser
# Algorithm for traverse voxels with a ray defined by two coordinate sets
# and calculated the path lenght of the ray in every traversed voxel. 
# Finally the intensity along the ray is integrated.
#
# (int_along_ray, voxels) = td_tdd(start_point,end_point,size_axes,origin_grid,grid_map)
#
# start_point = [x1 y1 z1]
# end_point = [x2 y2 z2]
# size_axes = [len_x len_y len_z]
# origin_grid = [voxel_x voxel_y voxel_z]
# grid_map(M,N,P)
#
# Example:
# TD_DDA([0.5 0.5 0.5],[2.5 5.5 10.5],[2 1 1],[0 0 0],grid)
#
# This function is largely following the algorithm:
# "A Fast Traversal Algorithm" by John Amanatides,
# Proc. Eurographics '87, Amsterdam, The Netherlands, August 1987, pp 1-10.
#
# Henning Osholm Sorensen, Risoe National Lab., Oct 5, 2006.

#from  Numeric import *

from numpy import float,int,inf,array,ones,zeros,linalg,dot,floor

def td_dda(p_start,p_end,axes,grid_origo,gridsize):
    p_start = array(p_start,float)
    p_end = array(p_end,float)
    grid_origo = array(grid_origo,float)
    gridsize = array(gridsize,float)
    axes = array(axes,float)
    zero = 1e-09
    final_out = False

    #Full path lenght in original (unscaled) coordinates
    ray =  p_start-p_end
    full_t = linalg.norm(ray)

    # normalize grid
    p_start = grid_origo + p_start/axes
    p_end = grid_origo + p_end/axes

    # Initialize variables
    t_total = 0
    nr_voxels = 0
    nextpix = zeros(3,int)
    delta = ones(3,int)
    t = zeros(3,float)
    t_one = zeros(3,float)
    voxel = []

    # the ray is defined r0 + t *r
    r0 = p_start
    r = p_end-r0
    t_max = linalg.norm(r) # Maximum ray path lenght in normalized coordinate system
    r = r/t_max

    #Scaling of path back to original system
    correction_fac = full_t/t_max

    # Check if r0 is outside grid 
    t_test = zeros(6,float)

    for i in range(3):
        if r0[i] < 0:
            t_test[i] = 1
        elif r0[i] > gridsize[i]-zero:
            t_test[i+3] = 1

    # If any coordinate is found to be outside grid above, determine the first point of ray intersecting the grid
    if max(t_test) > 0:
        x0 = array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[gridsize[0], 0, 0],[0, gridsize[1], 0],[0, 0, gridsize[2]]],float)
        normal = array([[1, 0, 0],[0, 1, 0],[0, 0, 1],[1, 0, 0],[0, 1, 0],[0, 0, 1]],float)
        for i in range(6):
            if t_test[i] == 1:
                if dot(normal[i][:],r) != 0.0:
                    t_test[i] = dot(normal[i][:],x0[i][:]-r0)/dot(normal[i][:],r)
                else:
                    voxel =  None
                    return voxel


        # As far as I see the longest path will be the one that hits a grid border,
        # whereas the shorter one just intersect the plane of a border, but outside the grid.
        t_to_grid = max(t_test)

        #Set new start point for ray at grid border
        r0 = r0+t_to_grid*r
        t_max = linalg.norm(p_end-r0)
        #For all safeties sake check the coordinates
        if (min(r0) < -zero ) or (r0[0] > gridsize[0]+zero) or (r0[1] > gridsize[1]+zero)  or (r0[2] > gridsize[2]+zero):
            voxel =  None
            return voxel
            #raise IOError,'2: The specified ray does not traverse the grid'
    else:
        x0 = array([[0, 0, 0],[0, 0, 0],[0, 0, 0],
	     	   [gridsize[0], 0, 0],[0, gridsize[1], 0],[0, 0, gridsize[2]]],float)
        normal = array([[1, 0, 0],[0, 1, 0],[0, 0, 1],
	       	 	[1, 0, 0],[0, 1, 0],[0, 0, 1]],float)
        t_test = zeros(3)
        for i in range(3):
            if dot(normal[i],r) != 0:
                if r[i] > 0:
                    t_test[i] = dot(normal[i],(x0[i]-r0))/dot(normal[i],r)
                else:
                    t_test[i] = dot(normal[i],(x0[i+3]-r0))/dot(normal[i],r)
            else:
                t_test[i] = inf;
        r0 = r0-min(abs(t_test))*r

    #The pixel where the ray originates	
    startpix = floor(r0)

    # Set step size and direction in x,y,z
    # find first intersection with voxel border
    for i in range(3):
        if r[i] == 0:
            t_one[i] = float('Inf') # Determine paths for stepping 1 in x,y,z respectively.
            t[i] =  float('Inf')    # Maybe add a check for r(i) = 0 not to divide by zero
        else:
            t_one[i] = abs(1/r[i])  # Determine paths for stepping 1 in x,y,z respectively.
            if r[i] > 0:
                t[i] = (floor(r0[i])+1-r0[i])/r[i]
            else:
                delta[i] = -1
                t[i] = (floor(r0[i])-r0[i])/r[i]

    # Find which voxel border is intersected next
    while final_out == False: # stops at grid border
        t_old =t
        if t[0] < t[1]:
            if t[0] < t[2]:
                #print "%i : x<y, but and x<z - 1" %nr_voxels
                pix = nextpix.copy()
                nextpix[0] = nextpix[0] + delta[0]
                t_voxel = t[0] - t_total
                t_total = t[0]
                t[0] = t[0] + t_one[0]
            else:
                #print "%i : x<y, but and z<x - 2" %nr_voxels
		pix = nextpix.copy()
                nextpix[2] = nextpix[2] + delta[2]
                t_voxel = t[2] - t_total
                t_total = t[2]
                t[2] = t[2] + t_one[2]
        else:
            if t[1] < t[2]:
                #print "%i : y<x, but and y<z - 3" %nr_voxels
		pix = nextpix.copy()
                nextpix[1] = nextpix[1] + delta[1]
                t_voxel = t[1] - t_total
                t_total = t[1]
                t[1] = t[1] + t_one[1]
            else:
                #print "%i : y<x, but and z<y - 4" %nr_voxels
		pix = nextpix.copy()
                nextpix[2] = nextpix[2] + delta[2]
                t_voxel = t[2] - t_total
                t_total = t[2]
                t[2] = t[2] + t_one[2]

        # Do not output if t_voxel is zero
        if t_voxel > zero:
            pix = pix + startpix
            if (min(pix) < 0) or \
	       (pix[0] > gridsize[0]-1) or \
	       (pix[1] > gridsize[1]-1) or \
	       (pix[2] > gridsize[2]-1):
                #print 'Final point outside grid. Stops tracing at grid border!'
                final_out = True
                break
            else:
                nr_voxels = nr_voxels + 1
                voxel.append([pix[0],pix[1],pix[2],t_voxel])

    # rescale the t_voxel to the original coordinatesystem
    voxel = array(voxel)
    voxel[:,3]=correction_fac*voxel[:,3]

    # Integrate intensity along ray

    return voxel

if __name__=='__main__':
    import time
    stime = time.clock()

    grid = ones((15,15,15))
    print 'grid size ',grid.shape
    start = [23.6 , 22, 120]
    end = [213.3, 216, 34]
    start = [-23 , -23, -23]
    end = [-21, -21, -21]
    axes =[1,1,1]
    origin = [0,0,0]
#    origin = array(grid.shape)/2.
    print 'origin ',origin
    pixlist = td_dda(start,end,axes,origin,grid.shape)

    #print int
    print 'pixlist:\n',pixlist
    print 'Time spend: %f sec' %(time.clock()-stime)
