USARBot.PropaSource

// ============================================================================
// The base class for the diffusion based source simulation.
//
// This class simulates the 2D diffusion at a timepoint. Since diffusion will 
// continue until it reachs the same density, we only simulate the density 
// distribution at a time point that is specified by the diffusuin range, PropaRange. 
// We use grid cell based diffusion to calculate the source density that is  
// stored in a byte array. 0~255 corresponds to 0 to initial density.
// It uses linear interpolation to find density at any point. The current class
// propagates only on the x-y plan. It assumes the same density in the vertical 
// direction in the vertical valid range. It's possible to propagate in 3D. But
// we need large array to store the densities.
// ============================================================================
class PropaSource extends AmbientSource
    abstract placeable;

struct Cell {
    var vector Location;
    var float  Density;
};

// The resolution -- the grid cell's size in meter.
var() float CellSize;
// The initial density at the source in percent.
var() float IniDensity;
// The environment density in percent.
var() float EnvDensity;
// The maximum diffusion range in meter.
var() float PropaRange;
// The valid range in vertical direction in meter
var() Range VertRange; 

var float uuCellSize;
var float uuPropaRange;
var Range uuVertRange; 

// The decline factor represents how the source propagates...
var float decline; 
// The factor used to convert byte (0~255) to density (0~IniDensity)
var float byteDens;
// The width of the density array
var int   width;
// The center (the source) of the density array
var int   center;

// The directions we will propagate 
var vector      dirs[8];
// The array stores the hits infor in the 8 dircetions
var byte        hits[8];
// The density array
var array<byte> densities;
// The frontier cells used in propagation calculation
var array<Cell> frontier;
var int         frLength;
// The valid range of the density array (density>0)
var RangeVector validRange;
// boolean indicates whether we have tailored the density array to store only
// the density within the valid range.
var bool        bReleased;
// For debug purpose
var bool        bDebug;

// Prepare the parameters and then call Propagation() to calculat densities.
// It then tailored the density array to save memory.
event PreBeginPlay()
{
    local int i;
    local Cell theCell;
    local float CalculationTime;

    Super.PreBeginPlay();
        
    center = int(PropaRange/CellSize);
    width = 2*center + 1;
    decline = (IniDensity-EnvDensity)/uuPropaRange;
    byteDens = 255/(IniDensity-EnvDensity);
    
    Clock(CalculationTime);

    // init densities array
    for (i=0;i<width*width;i++)
        densities[i] = 0;
        
    frontier[0] = theCell;
    frontier[0].Location = Location;
    frontier[0].Density = IniDensity;
    validRange.X.Min = Location.X;
    validRange.X.Max = Location.X;
    validRange.Y.Min = Location.Y;
    validRange.Y.Max = Location.Y;
    frLength = 1;
    setDensity(frontier[0]);
    
    Propagation();
    releaseDens();
    
    UnClock(CalculationTime);
    log("Calculate Density for "$ Name $" [Size:"@densities.length@"byte Time:"@CalculationTime@"ms]");
}

// for testing purpose
function PostBeginPlay()
{
    //local vector loc;
    //local string str;
     
    if (bDebug)
        dumpDensities();
    
    // Testing the densities
    //loc.z = Location.z;
    //loc.y = validRange.Y.Min;
    //while (loc.y<=validRange.Y.Max) {
    //	str = "";
    //	loc.x = validRange.X.Min;
    //	while (loc.x<=validRange.X.Max) {
    //		str = str@getDensity(loc)*ByteDens;
    //		loc.x += uuCellSize/4;
    //	}
    //	log(str);
    //	loc.y += uuCellSize/4;
    //}
}

function dumpDensities()
{
    local int i, j, w, h;
    local string str;
    
    log(Name@"The densities:");
    h = int((validRange.Y.Max-validRange.Y.Min)/uuCellSize)+1;
    w = int((validRange.X.Max-validRange.X.Min)/uuCellSize)+1;
    for (i=0; i<h; i++) {
        str = " ";
        for (j=0; j<w; j++)
            str = str@densities[j+i*w];
        log(Name@str);
    }
}

function ConvertParam(USARConverter converter)
{
    if (converter==None) {
        uuCellSize = CellSize;
        uuPropaRange = PropaRange;
        uuVertRange = VertRange;
    } else {
        uuCellSize = converter.LengthToUU(CellSize);
        uuPropaRange = converter.LengthToUU(PropaRange);
        uuVertRange.Min = converter.LengthToUU(VertRange.Min);
        uuVertRange.Max = converter.LengthToUU(VertRange.Max);
    }
}

// Tailored density array to save memory
function releaseDens()
{
    local int pos, step, len1, len2;
    local float y;
    
    //clear dirty memory
    frontier.remove(0,frontier.length);
    
    // trim densities array
    len1 = int((Location.Y + uuPropaRange - validRange.Y.Max)/uuCellSize)*width; // bottom
    len2 = int((Location.X + uuPropaRange - validRange.X.Max)/uuCellSize); // right
    pos = densities.length - len1 - len2;
    densities.remove(pos,len1+len2);
    len1 = int((validRange.X.Min - Location.X + uuPropaRange)/uuCellSize); // left
    step = int((validRange.X.Max - validRange.X.Min)/uuCellSize)+1;
    for (y = validRange.Y.Max; y>validRange.Y.Min; y-=uuCellSize) {
        pos -= step + len1 + len2;
        densities.remove(pos,len1+len2);
    }
    len2 = int((validRange.Y.Min - Location.Y + uuPropaRange)/uuCellSize)*width;
    densities.remove(0,len2+len1);
    
    // we are now using the tailored array
    bReleased = True;
    width = step;
}

function setDensity(Cell theCell)
{
    local int x, y;
    
    x = int((theCell.Location.X - Location.X)/uuCellSize)+center;
    y = int((theCell.Location.Y - Location.Y)/uuCellSize)+center;
    densities[x+y*width] = byte((theCell.Density-EnvDensity)*byteDens);
    
    if (theCell.Location.X > validRange.X.Max) 
        validRange.X.Max = theCell.Location.X;
    else if (theCell.Location.X < validRange.X.Min) 
        validRange.X.Min = theCell.Location.X;
    if (theCell.Location.Y > validRange.Y.Max) 
        validRange.Y.Max = theCell.Location.Y;
    else if (theCell.Location.Y < validRange.Y.Min) 
        validRange.Y.Min = theCell.Location.Y;
}

function setDensity2(vector loc, float density)
{
    local int x, y;
    
    x = int((loc.X - Location.X)/uuCellSize)+center;
    y = int((loc.Y - Location.Y)/uuCellSize)+center;
    densities[x+y*width] = byte((density-EnvDensity)*byteDens);

    if (loc.X > validRange.X.Max) 
        validRange.X.Max = loc.X;
    else if (loc.X < validRange.X.Min) 
        validRange.X.Min = loc.X;
    if (loc.Y > validRange.Y.Max) 
        validRange.Y.Max = loc.Y;
    else if (loc.Y < validRange.Y.Min) 
        validRange.Y.Min = loc.Y;
}

// If bReleased=false, we directly return the density. This case only happens
// during the propagation computation. For sensor query, bReleased should be 
// true, and we will check the validation and return the interpolation data.
function float getDensity(Vector loc)
{
    local int x, y, x0, y0, i;
    local float dx, dy, den, den2, halfCell;
    local Vector ref;
    local array<Vector> refs;
    
    if (!bReleased) {
        x = int((loc.X - Location.X)/uuCellSize)+center;
        y = int((loc.Y - Location.Y)/uuCellSize)+center;
        if (x<0 || x>=width || y<0 || y>=width)
            return EnvDensity;
        else
            return densities[x+y*width]/byteDens+EnvDensity;
    }
    
    halfCell = uuCellSize/2;
    if (loc.X<validRange.X.Min-halfCell ||
        loc.X>validRange.X.Max+halfCell ||
        loc.Y<validRange.Y.Min-halfCell ||
        loc.Y>validRange.Y.Max+halfCell ||
        loc.Z<Location.z+uuVertRange.Min ||
        loc.Z>Location.z+uuVertRange.Max)
        return EnvDensity;
        
    x0 = int((loc.X - validRange.X.Min)/uuCellSize+0.5);
    y0 = int((loc.Y - validRange.Y.Min)/uuCellSize+0.5);
    
    dx = loc.X - validRange.X.Min - x0*uuCellSize;
    dy = loc.Y - validRange.Y.Min - y0*uuCellSize;
    //log(dx@dy@loc);
    
    x = x0;
    y = y0;
    ref.X = validRange.X.Min + x*uuCellSize;
    ref.Y = validRange.Y.Min + y*uuCellSize;
    ref.z = loc.z;
    if (FastTrace(ref,loc)) {
        ref.Z = densities[x+y*width];
        refs[i++] = ref;
    }
    
    x = x + sign(dx);
    if (x>=0 && x<width) {
        ref.X = validRange.X.Min + x*uuCellSize;
        ref.Y = validRange.Y.Min + y*uuCellSize;
        ref.z = loc.z;
        if (FastTrace(ref,loc) && sign(dx)!=0) {
            ref.Z = densities[x+y*width];
            refs[i++] = ref;
        }
        
        y = y + sign(dy);
        if (y>=0 && y<width) {  
            ref.X = validRange.X.Min + x*uuCellSize;
            ref.Y = validRange.Y.Min + y*uuCellSize;
            ref.z = loc.z;
            if (FastTrace(ref,loc) && sign(dx)!=0 && sign(dy)!=0) {
                ref.Z = densities[x+y*width];
                refs[i++] = ref;
            }
        }
    }
    
    x = x0;
    if (y>=0 && y<width) {  
        ref.X = validRange.X.Min + x*uuCellSize;
        ref.Y = validRange.Y.Min + y*uuCellSize;
        ref.Z = loc.z;
        if (FastTrace(ref,loc) && sign(dy)!=0) {
            ref.Z = densities[x+y*width];
            refs[i++] = ref;
        }
    }
    
    switch(refs.length) {
        case 1:
            //log("["$1$"]");
            return refs[0].z/byteDens+EnvDensity;
        case 2:
            //log("["$2$"]"@refs[0]@refs[1]);
            den = ((refs[1].x-refs[0].x)*(loc.x-refs[0].x)+(refs[1].y-refs[0].y)*(loc.y-refs[0].y))/
                  ((refs[1].x-refs[0].x)*(refs[1].x-refs[0].x)+(refs[1].y-refs[0].y)*(refs[1].y-refs[0].y));
            den = den * (refs[1].Z-refs[0].Z) + refs[0].Z;
            return den/byteDens+EnvDensity;
        case 3:
            //log("["$3$"]"@refs[0]@refs[1]@refs[2]);
            den = ((refs[0].y-refs[2].y)*(refs[1].z-refs[2].z) - (refs[1].y-refs[2].y)*(refs[0].z-refs[2].z)) * (loc.x - refs[2].x) -
                  ((refs[0].x-refs[2].x)*(refs[1].z-refs[2].z) - (refs[1].x-refs[2].x)*(refs[0].z-refs[2].z)) * (loc.y - refs[2].y);
            den = -den/((refs[0].x-refs[2].x)*(refs[1].y-refs[2].y) - (refs[1].x-refs[2].x)*(refs[0].y-refs[2].y));
            den = den + refs[2].z;
            return den/byteDens+EnvDensity;
        case 4:
            //log("["$4$"]"@refs[0]@refs[1]@refs[2]@refs[3]);
            den = ((refs[0].y-refs[2].y)*(refs[1].z-refs[2].z) - (refs[1].y-refs[2].y)*(refs[0].z-refs[2].z)) * (loc.x - refs[2].x) -
                  ((refs[0].x-refs[2].x)*(refs[1].z-refs[2].z) - (refs[1].x-refs[2].x)*(refs[0].z-refs[2].z)) * (loc.y - refs[2].y);
            den = -den/((refs[0].x-refs[2].x)*(refs[1].y-refs[2].y) - (refs[1].x-refs[2].x)*(refs[0].y-refs[2].y));
            den = den + refs[2].z;

            den2 = ((refs[0].y-refs[2].y)*(refs[3].z-refs[2].z) - (refs[3].y-refs[2].y)*(refs[0].z-refs[2].z)) * (loc.x - refs[2].x) -
                   ((refs[0].x-refs[2].x)*(refs[3].z-refs[2].z) - (refs[3].x-refs[2].x)*(refs[0].z-refs[2].z)) * (loc.y - refs[2].y);
            den2 = -den2/((refs[0].x-refs[2].x)*(refs[3].y-refs[2].y) - (refs[3].x-refs[2].x)*(refs[0].y-refs[2].y));
            den2 = den2 + refs[2].z;
            den += den2;

            den2 = ((refs[0].y-refs[1].y)*(refs[3].z-refs[1].z) - (refs[3].y-refs[1].y)*(refs[0].z-refs[1].z)) * (loc.x - refs[1].x) -
                   ((refs[0].x-refs[1].x)*(refs[3].z-refs[1].z) - (refs[3].x-refs[1].x)*(refs[0].z-refs[1].z)) * (loc.y - refs[1].y);
            den2 = -den2/((refs[0].x-refs[1].x)*(refs[3].y-refs[1].y) - (refs[3].x-refs[1].x)*(refs[0].y-refs[1].y));
            den2 = den2 + refs[1].z;
            den += den2;

            den2 = ((refs[2].y-refs[1].y)*(refs[3].z-refs[1].z) - (refs[3].y-refs[1].y)*(refs[2].z-refs[1].z)) * (loc.x - refs[1].x) -
                   ((refs[2].x-refs[1].x)*(refs[3].z-refs[1].z) - (refs[3].x-refs[1].x)*(refs[2].z-refs[1].z)) * (loc.y - refs[1].y);
            den2 = -den2/((refs[2].x-refs[1].x)*(refs[3].y-refs[1].y) - (refs[3].x-refs[1].x)*(refs[2].y-refs[1].y));
            den2 = den2 + refs[1].z;
            den += den2;

            return den/4/byteDens+EnvDensity;
        default:
            return densities[x0+y0*width]/byteDens+EnvDensity;
    }
}

function float sign(float f)
{
    if (f>0) return 1;
    else if (f<0) return -1;
    else return 0;
}

// check the hit situation in the 8 directions
function checkHits(vector loc, float range)
{
    local int i;

    for (i=0;i<8;i++) {
        if (FastTrace(loc + range*dirs[i], loc)) 
            hits[i]=0;
        else
            hits[i]=1;
    }
}

// The decline function. We can override this function to simulate different
// propagation style.
function float calcDens(float iniDens, float length)
{
    return iniDens - decline*length;
}

// The propagation simulation function
function Propagation()
{
    local int i, j;
    local array<Cell> myFrt;
    local Cell theCell;
    local int myFrtLen;
    local vector myLoc;
    
    myFrtLen = 0;
    for (i=0; i<frLength; i++) {
        checkHits(frontier[i].Location, uuCellSize);
        if (bDebug)
            log(Name@">>"@frontier[i].Location@frontier[i].Density);
        for (j=0; j<8; j++) {
            myLoc = frontier[i].Location + dirs[j]*uuCellSize;
            
            if (getDensity(myLoc)==0) {
                if (hits[j]==0) {
                    myFrt[myFrtLen] = theCell;
                    myFrt[myFrtLen].Location = myLoc;
                    if (j%2==0)
                        myFrt[myFrtLen].Density = calcDens(frontier[i].Density,uuCellSize);
                    else
                        myFrt[myFrtLen].Density = calcDens(frontier[i].Density,uuCellSize*1.414);
                    setDensity(myFrt[myFrtLen]);
                    if (bDebug)
                        log(Name@"  "@myFrt[myFrtLen].Location@myFrt[myFrtLen].Density);
                    myFrtLen += 1;
                }
            }
        }
    }
    
    frLength = 0;
    for (i=0; i<myFrtLen; i++) {
        if (Abs(myFrt[i].Location.X - Location.X)<uuPropaRange &&
            Abs(myFrt[i].Location.Y - Location.Y)<uuPropaRange) {
            frontier[i] = theCell;
            frontier[i].Location = myFrt[i].Location;
            frontier[i].Density = myFrt[i].Density;
            frLength += 1;
        }
    }
    
    if (frLength>0)
        Propagation();
}

defaultproperties
{
    dirs(0)=(X= 1,Y= 0,Z=0)
    dirs(1)=(X= 1,Y= 1,Z=0)
    dirs(2)=(X= 0,Y= 1,Z=0)
    dirs(3)=(X= 1,Y=-1,Z=0)
    dirs(4)=(X= 0,Y=-1,Z=0)
    dirs(5)=(X=-1,Y=-1,Z=0)
    dirs(6)=(X=-1,Y= 0,Z=0)
    dirs(7)=(X=-1,Y= 1,Z=0)
    
    bDebug=False
    SourceType="propagation"
    CellSize=0.5
    IniDensity=100
    EnvDensity=0
    PropaRange=20
    VertRange=(Min=-1,Max=1)
}