protected void Init(
int dimension_cells,
int dimension_cells_vertical,
int cellSize_mm,
int localisationRadius_mm,
int maxMappingRange_mm,
float vacancyWeighting)
{
cell = new particleGridCellBase[dimension_cells][][];
for (int x = 0; x < dimension_cells; x++)
cell[x] = new particleGridCellBase[dimension_cells][];
localisation_test = new bool[dimension_cells][];
for (int x = 0; x < dimension_cells; x++)
localisation_test[x] = new bool[dimension_cells];
this.dimension_cells = dimension_cells;
this.dimension_cells_vertical = dimension_cells_vertical;
this.cellSize_mm = cellSize_mm;
this.localisation_search_cells = localisationRadius_mm / cellSize_mm;
this.max_mapping_range_cells = maxMappingRange_mm / cellSize_mm;
this.vacancy_weighting = vacancyWeighting;
// make a lookup table for gaussians - saves doing a lot of floating point maths
gaussianLookup = stereoModel.createHalfGaussianLookup(10);
// create a lookup table for log odds calculations, to avoid having
// to run slow Log calculations at runtime
LogOdds = probabilities.CreateLogOddsLookup(LOG_ODDS_LOOKUP_LEVELS);
}
/// <summary>
/// distill an individual grid particle
/// </summary>
/// <param name="hypothesis">the grid particle to be distilled</param>
public void Distill(particleGridCell hypothesis)
{
int z = hypothesis.z;
// create the distilled array
if (distilled == null)
{
distilled = new particleGridCellBase[Hypothesis.Length];
}
bool initialised = false;
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
initialised = true;
}
// update an existing distilled value
distilled[z].probabilityLogOdds += hypothesis.probabilityLogOdds;
// and update the distilled colour value
for (int col = 2; col >= 0; col--)
{
if (initialised)
{
distilled[z].colour[col] = hypothesis.colour[col];
}
else
{
distilled[z].colour[col] = (byte)((hypothesis.colour[col] +
distilled[z].colour[col]) / 2);
}
}
}
/// <summary>
/// distill a given probability value
/// </summary>
/// <param name="z">z coordinate</param>
/// <param name="probability">probability value in the range 0.0-1.0</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public void Distill(
int z,
float probability,
int r, int g, int b)
{
// create the distilled array
if (distilled == null)
{
distilled = new particleGridCellBase[Hypothesis.Length];
}
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
}
// update an existing distilled value
distilled[z].probabilityLogOdds = probabilities.LogOdds(probability);
distilled[z].colour[0] = (byte)r;
distilled[z].colour[1] = (byte)g;
distilled[z].colour[2] = (byte)b;
}
public void LoadTile(byte[] data)
{
// read the bounding box
int array_index = 0;
const int int32_bytes = 4;
int tx = BitConverter.ToInt32(data, 0);
int ty = BitConverter.ToInt32(data, int32_bytes);
int bx = BitConverter.ToInt32(data, int32_bytes * 2);
int by = BitConverter.ToInt32(data, int32_bytes * 3);
array_index = int32_bytes * 4;
// dimensions of the box
int w1 = bx - tx;
int w2 = by - ty;
//Read binary index as a byte array
int no_of_bits = w1 * w2;
int no_of_bytes = no_of_bits / 8;
if (no_of_bytes * 8 < no_of_bits) no_of_bytes++;
byte[] indexData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
indexData[i] = data[array_index + i];
bool[] binary_index = ArrayConversions.ToBooleanArray(indexData);
array_index += no_of_bytes;
int n = 0;
int occupied_cells = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (binary_index[n])
{
cell[x][y] = new occupancygridCellMultiHypothesis(dimension_cells_vertical);
occupied_cells++;
}
n++;
}
}
if (occupied_cells > 0)
{
const int float_bytes = 4;
// read occupancy values
no_of_bytes = occupied_cells * dimension_cells_vertical * float_bytes;
byte[] occupancyData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
occupancyData[i] = data[array_index + i];
array_index += no_of_bytes;
float[] occupancy = ArrayConversions.ToFloatArray(occupancyData);
// read colour values
no_of_bytes = occupied_cells * dimension_cells_vertical * 3;
byte[] colourData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
colourData[i] = data[array_index + i];
array_index += no_of_bytes;
// insert the data into the grid
n = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (cell[x][y] != null)
{
particleGridCellBase[] distilled = new particleGridCellBase[dimension_cells_vertical];
for (int z = 0; z < dimension_cells_vertical; z++)
{
// set the probability value
int index = (n * dimension_cells_vertical) + z;
float probLogOdds = occupancy[index];
if (probLogOdds != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
// create a distilled grid particle
distilled[z] = new particleGridCellBase();
distilled[z].probabilityLogOdds = probLogOdds;
// set the colour
distilled[z].colour = new byte[3];
for (int col = 0; col < 3; col++)
distilled[z].colour[col] = colourData[(index * 3) + col];
}
}
// insert the distilled particles into the grid cell
cell[x][y].SetDistilledValues(distilled);
// update the navigable space
updateNavigableSpace(null, x, y);
n++;
}
}
}
}
}
/// <summary>
/// sets distilled probability values
/// This is used when loading a grid from file
/// </summary>
/// <param name="distilled"></param>
public void SetDistilledValues(particleGridCellBase[] distilled)
{
this.distilled = distilled;
}
/// <summary>
/// distill an individual grid particle
/// </summary>
/// <param name="hypothesis">the grid particle to be distilled</param>
public void Distill(particleGridCell hypothesis)
{
int z = hypothesis.z;
// create the distilled array
if (distilled == null)
distilled = new particleGridCellBase[Hypothesis.Length];
bool initialised = false;
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
initialised = true;
}
// update an existing distilled value
distilled[z].probabilityLogOdds += hypothesis.probabilityLogOdds;
// and update the distilled colour value
for (int col = 2; col >= 0; col--)
{
if (initialised)
distilled[z].colour[col] = hypothesis.colour[col];
else
distilled[z].colour[col] = (byte)((hypothesis.colour[col] +
distilled[z].colour[col]) / 2);
}
}
/// <summary>
/// distill a given probability value
/// </summary>
/// <param name="z">z coordinate</param>
/// <param name="probability">probability value in the range 0.0-1.0</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public void Distill(
int z,
float probability,
int r, int g, int b)
{
// create the distilled array
if (distilled == null)
distilled = new particleGridCellBase[Hypothesis.Length];
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
}
// update an existing distilled value
distilled[z].probabilityLogOdds = probabilities.LogOdds(probability);
distilled[z].colour[0] = (byte)r;
distilled[z].colour[1] = (byte)g;
distilled[z].colour[2] = (byte)b;
}
/// <summary>
/// inserts the given ray into the grid
/// There are three components to the sensor model used here:
/// two areas determining the probably vacant area and one for
/// the probably occupied space
/// </summary>
/// <param name="ray">ray object to be inserted into the grid</param>
/// <param name="sensormodel_lookup">the sensor model to be used</param>
/// <param name="left_camera_location">location and pose of the left camera</param>
/// <param name="right_camera_location">location and pose of the right camera</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
/// <returns>matching probability, expressed as log odds</returns>
public float Insert(
evidenceRay ray,
rayModelLookup sensormodel_lookup,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
// some constants to aid readability
const int OCCUPIED_SENSORMODEL = 0;
const int VACANT_SENSORMODEL_LEFT_CAMERA = 1;
const int VACANT_SENSORMODEL_RIGHT_CAMERA = 2;
const int X_AXIS = 0;
const int Y_AXIS = 1;
float inverse_cellSize_mm = 1.0f / cellSize_mm;
// which disparity index in the lookup table to use
// we multiply by 2 because the lookup is in half pixel steps
int sensormodel_index = (int)Math.Round(ray.disparity * 2);
// the initial models are blank, so just default to the one disparity pixel model
bool small_disparity_value = false;
if (sensormodel_index < 2)
{
sensormodel_index = 2;
small_disparity_value = true;
}
// beyond a certain disparity the ray model for occupied space
// is always only going to be only a single grid cell
if (sensormodel_index >= sensormodel_lookup.probability.GetLength(0))
sensormodel_index = sensormodel_lookup.probability.GetLength(0) - 1;
float xdist_mm=0, ydist_mm=0, zdist_mm=0, xx_mm=0, yy_mm=0, zz_mm=0;
float occupied_dx = 0, occupied_dy = 0, occupied_dz = 0;
//float intersect_x = 0, intersect_y = 0, intersect_z = 0;
float centre_prob = 0, prob = 0, prob_localisation = 0; // probability values at the centre axis and outside
float matchingScore = NO_OCCUPANCY_EVIDENCE; // total localisation matching score
int rayWidth = 0; // widest point in the ray in cells
int widest_point; // widest point index
int step_size = 1;
// ray width at the fattest point in cells
rayWidth = (int)Math.Round(ray.width / (cellSize_mm * 2));
// calculate the centre position of the grid in millimetres
int half_grid_width_mm = dimension_cells * cellSize_mm / 2;
//int half_grid_width_vertical_mm = dimension_cells_vertical * cellSize_mm / 2;
int grid_centre_x_mm = (int)(x - half_grid_width_mm);
int grid_centre_y_mm = (int)(y - half_grid_width_mm);
int grid_centre_z_mm = (int)z;
int max_dimension_cells = dimension_cells - rayWidth;
// in turbo mode only use a single vacancy ray
int max_modelcomponent = VACANT_SENSORMODEL_RIGHT_CAMERA;
if (TurboMode) max_modelcomponent = VACANT_SENSORMODEL_LEFT_CAMERA;
if (localiseOnly) max_modelcomponent = OCCUPIED_SENSORMODEL;
float[][] sensormodel_lookup_probability = sensormodel_lookup.probability;
// consider each of the three parts of the sensor model
for (int modelcomponent = OCCUPIED_SENSORMODEL; modelcomponent <= max_modelcomponent; modelcomponent++)
{
// the range from the cameras from which insertion of data begins in grid cells
// for vacancy rays this will be zero, but will be non-zero for the occupancy area
int starting_range_cells = 0;
switch (modelcomponent)
{
case OCCUPIED_SENSORMODEL:
{
// distance between the beginning and end of the probably
// occupied area
occupied_dx = ray.vertices[1].x - ray.vertices[0].x;
occupied_dy = ray.vertices[1].y - ray.vertices[0].y;
occupied_dz = ray.vertices[1].z - ray.vertices[0].z;
//intersect_x = ray.vertices[0].x + (occupied_dx * ray.fattestPoint);
//intersect_y = ray.vertices[0].y + (occupied_dy * ray.fattestPoint);
//intersect_z = ray.vertices[0].z + (occupied_dz * ray.fattestPoint);
xdist_mm = occupied_dx;
ydist_mm = occupied_dy;
zdist_mm = occupied_dz;
// begin insertion at the beginning of the
// probably occupied area
xx_mm = ray.vertices[0].x;
yy_mm = ray.vertices[0].y;
zz_mm = ray.vertices[0].z;
break;
}
case VACANT_SENSORMODEL_LEFT_CAMERA:
{
// distance between the left camera and the left side of
// the probably occupied area of the sensor model
xdist_mm = ray.vertices[0].x - left_camera_location.x;
ydist_mm = ray.vertices[0].y - left_camera_location.y;
zdist_mm = ray.vertices[0].z - left_camera_location.z;
// begin insertion from the left camera position
xx_mm = left_camera_location.x;
yy_mm = left_camera_location.y;
zz_mm = left_camera_location.z;
//step_size = 2;
break;
}
case VACANT_SENSORMODEL_RIGHT_CAMERA:
{
// distance between the right camera and the right side of
// the probably occupied area of the sensor model
xdist_mm = ray.vertices[0].x - right_camera_location.x;
ydist_mm = ray.vertices[0].y - right_camera_location.y;
zdist_mm = ray.vertices[0].z - right_camera_location.z;
// begin insertion from the right camera position
xx_mm = right_camera_location.x;
yy_mm = right_camera_location.y;
zz_mm = right_camera_location.z;
//step_size = 2;
break;
}
}
// which is the longest axis ?
int longest_axis = X_AXIS;
float longest = Math.Abs(xdist_mm);
if (Math.Abs(ydist_mm) > longest)
{
// y has the longest length
longest = Math.Abs(ydist_mm);
longest_axis = Y_AXIS;
}
// ensure that the vacancy model does not overlap
// the probably occupied area
// This is crude and could potentially leave a gap
// longest -= ray.width;
int steps = (int)(longest * inverse_cellSize_mm);
if (steps < 1) steps = 1;
float inverse_steps = 1.0f / steps;
// calculate the range from the cameras to the start of the ray in grid cells
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
if (longest_axis == Y_AXIS)
starting_range_cells = (int)((ray.vertices[0].y - ray.observedFrom.y) * inverse_cellSize_mm);
else
starting_range_cells = (int)((ray.vertices[0].x - ray.observedFrom.x) * inverse_cellSize_mm);
// always positive
if (starting_range_cells < 0) starting_range_cells = -starting_range_cells;
// what is the widest point of the ray in cells
widest_point = (int)(ray.fattestPoint * steps);
}
else
{
// what is the widest point of the ray in cells
widest_point = steps;
}
// calculate increment values in millimetres
float x_incr_mm = xdist_mm * inverse_steps;
float y_incr_mm = ydist_mm * inverse_steps;
float z_incr_mm = zdist_mm * inverse_steps;
// step through the ray, one grid cell at a time
int grid_step = 0;
while (grid_step < steps)
{
// is this position inside the maximum mapping range
bool withinMappingRange = true;
if (grid_step + starting_range_cells > max_mapping_range_cells)
{
withinMappingRange = false;
if ((grid_step==0) && (modelcomponent == OCCUPIED_SENSORMODEL))
{
grid_step = steps;
modelcomponent = 9999;
}
}
// calculate the width of the ray in cells at this point
// using a diamond shape ray model
int ray_wdth = 0;
if (rayWidth > 0)
{
if (grid_step < widest_point)
ray_wdth = grid_step * rayWidth / widest_point;
else
{
if (!small_disparity_value)
// most disparity values tail off to some point in the distance
ray_wdth = (steps - grid_step + widest_point) * rayWidth / (steps - widest_point);
else
// for very small disparity values the ray model has an infinite tail
// and maintains its width after the widest point
ray_wdth = rayWidth;
}
}
// localisation rays are wider, to enable a more effective matching score
// which is not too narrowly focussed and brittle
int ray_wdth_localisation = ray_wdth + 1; //localisation_search_cells;
xx_mm += x_incr_mm*step_size;
yy_mm += y_incr_mm*step_size;
zz_mm += z_incr_mm*step_size;
// convert the x millimetre position into a grid cell position
int x_cell = (int)(((xx_mm - grid_centre_x_mm) * inverse_cellSize_mm) + 0.5f);
if ((x_cell > ray_wdth_localisation) && (x_cell < dimension_cells - ray_wdth_localisation))
{
// convert the y millimetre position into a grid cell position
int y_cell = (int)(((yy_mm - grid_centre_y_mm) * inverse_cellSize_mm) + 0.5f);
if ((y_cell > ray_wdth_localisation) && (y_cell < dimension_cells - ray_wdth_localisation))
{
// convert the z millimetre position into a grid cell position
int z_cell = (int)(((zz_mm - grid_centre_z_mm) * inverse_cellSize_mm) + 0.5f);
if ((z_cell >= 0) && (z_cell < dimension_cells_vertical))
{
int x_cell2 = x_cell;
int y_cell2 = y_cell;
// get the probability at this point
// for the central axis of the ray using the inverse sensor model
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
centre_prob = 0.5f + (sensormodel_lookup_probability[sensormodel_index][grid_step] * 0.5f);
}
else
{
// calculate the probability from the vacancy model
centre_prob = vacancyFunction(grid_step * inverse_steps, steps);
}
// width of the localisation ray
for (int width = -ray_wdth_localisation; width <= ray_wdth_localisation; width++)
{
// is the width currently inside the mapping area of the ray ?
bool isInsideMappingRayWidth = false;
if ((width >= -ray_wdth) && (width <= ray_wdth))
isInsideMappingRayWidth = true;
// adjust the x or y cell position depending upon the
// deviation from the main axis of the ray
if (longest_axis == Y_AXIS)
x_cell2 = x_cell + width;
else
y_cell2 = y_cell + width;
// probability at the central axis
prob = centre_prob;
prob_localisation = centre_prob;
// probabilities are symmetrical about the axis of the ray
// this multiplier implements a gaussian distribution around the centre
if (width != 0) // don't bother if this is the centre of the ray
{
if (ray_wdth > 0)
{
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
prob_localisation = 0.5f + ((prob_localisation - 0.5f) * gaussianLookup[Math.Abs(width) * 9 / ray_wdth_localisation]);
// the probability used for narrower mapping rays
if (isInsideMappingRayWidth)
prob = 0.5f + ((prob - 0.5f) * gaussianLookup[Math.Abs(width) * 9 / ray_wdth]);
}
else
{
prob_localisation *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth_localisation];
if (isInsideMappingRayWidth) prob *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth];
}
}
}
if ((withinMappingRange) &&
(localiseOnly))
{
// only localise using occupancy, not vacancy
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
// test
localisation_test[x_cell2][y_cell2] = true;
if (cell[x_cell2][y_cell2] != null)
{
if (cell[x_cell2][y_cell2][z_cell] != null)
{
//if (cell[x_cell2][y_cell2][z_cell].probabilityLogOdds > 0)
{
// update the matching score, by combining the probability
// of the grid cell with the probability from the localisation ray
float score = NO_OCCUPANCY_EVIDENCE;
if (longest_axis == X_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, prob_localisation, ray.colour);
if (longest_axis == Y_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, prob_localisation, ray.colour);
if (score != NO_OCCUPANCY_EVIDENCE)
{
if (matchingScore != NO_OCCUPANCY_EVIDENCE)
matchingScore += score;
else
matchingScore = score;
}
}
}
}
}
}
if ((isInsideMappingRayWidth) &&
(withinMappingRange) &&
(!localiseOnly))
{
// generate a grid cell if necessary
if (cell[x_cell2][y_cell2] == null)
cell[x_cell2][y_cell2] = new particleGridCellBase[dimension_cells_vertical];
if (cell[x_cell2][y_cell2][z_cell] == null)
cell[x_cell2][y_cell2][z_cell] = new particleGridCellBase();
particleGridCellBase c = cell[x_cell2][y_cell2][z_cell];
c.probabilityLogOdds += probabilities.LogOdds(prob);
c.colour = ray.colour; // this is simplistic, but we'll live with it
}
}
}
//else grid_step = steps; // its the end of the ray, break out of the loop
}
//else grid_step = steps; // its the end of the ray, break out of the loop
}
//else grid_step = steps; // time to bail out chaps!
grid_step += step_size;
}
}
return (matchingScore);
}
/// <summary>
/// inserts a simulated doorway into the grid
/// </summary>
/// <param name="tx_cell">start x cell coordinate</param>
/// <param name="ty_cell">start y cell coordinate</param>
/// <param name="bx_cell">end x cell coordinate</param>
/// <param name="by_cell">end y cell coordinate</param>
/// <param name="wall_height_cells">height of the wall in cells</param>
/// <param name="door_height_cells">height of the doorway in cells</param>
/// <param name="door_width_cells">width of the doorway in cells</param>
/// <param name="thickness_cells">thickness of the wall in cells</param>
/// <param name="probability_variance">variation of probabilities, typically less than 0.2</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public override void InsertDoorwayCells(
int tx_cell, int ty_cell,
int bx_cell, int by_cell,
int wall_height_cells,
int door_height_cells,
int door_width_cells,
int thickness_cells,
float probability_variance,
int r, int g, int b)
{
Random rnd = new Random(0);
if (wall_height_cells >= dimension_cells_vertical)
wall_height_cells = dimension_cells_vertical - 1;
if (thickness_cells < 1) thickness_cells = 1;
if (door_height_cells > wall_height_cells)
door_height_cells = wall_height_cells;
int dx = bx_cell - tx_cell;
int dy = by_cell - ty_cell;
int length_cells = (int)Math.Sqrt(dx*dx + dy*dy);
int door_start_index = (length_cells/2) - (door_width_cells/2);
int door_end_index = door_start_index + door_width_cells;
for (int i = 0; i < length_cells; i++)
{
int x_cell = tx_cell + (i * dx / length_cells);
if ((x_cell > -1) && (x_cell < dimension_cells))
{
int y_cell = ty_cell + (i * dy / length_cells);
if ((y_cell > -1) && (y_cell < dimension_cells))
{
if (cell[x_cell][y_cell] == null)
cell[x_cell][y_cell] = new particleGridCellBase[dimension_cells_vertical];
for (int z_cell = 0; z_cell < wall_height_cells; z_cell++)
{
if (cell[x_cell][y_cell][z_cell] == null)
cell[x_cell][y_cell][z_cell] = new particleGridCellBase();
float prob = 0.8f + ((float)rnd.NextDouble() * probability_variance * 2) - probability_variance;
if (prob > 0.99f) prob = 0.99f;
if (prob < 0.5f) prob = 0.5f;
if ((i >= door_start_index) &&
(i <= door_end_index) &&
(z_cell < door_height_cells))
{
prob = 0.2f + ((float)rnd.NextDouble() * probability_variance * 2) - probability_variance;
if (prob > 0.4f) prob = 0.4f;
if (prob < 0.01f) prob = 0.01f;
}
particleGridCellBase c = cell[x_cell][y_cell][z_cell];
c.probabilityLogOdds = probabilities.LogOdds(prob);
c.colour = new byte[3];
c.colour[0] = (byte)r;
c.colour[1] = (byte)g;
c.colour[1] = (byte)b;
}
}
}
}
}
/// <summary>
/// inserts a simulated wall into the grid
/// </summary>
/// <param name="tx_cell">start x cell coordinate</param>
/// <param name="ty_cell">start y cell coordinate</param>
/// <param name="bx_cell">end x cell coordinate</param>
/// <param name="by_cell">end y cell coordinate</param>
/// <param name="height_cells">height of the wall in cells</param>
/// <param name="thickness_cells">thickness of the wall in cells</param>
/// <param name="probability_variance">variation of probabilities, typically less than 0.2</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public override void InsertWallCells(
int tx_cell, int ty_cell,
int bx_cell, int by_cell,
int height_cells,
int thickness_cells,
float probability_variance,
int r, int g, int b)
{
Random rnd = new Random(0);
if (height_cells >= dimension_cells_vertical)
height_cells = dimension_cells_vertical - 1;
if (thickness_cells < 1) thickness_cells = 1;
int dx = bx_cell - tx_cell;
int dy = by_cell - ty_cell;
int length_cells = (int)Math.Sqrt(dx*dx + dy*dy);
for (int i = 0; i < length_cells; i++)
{
int x_cell = tx_cell + (i * dx / length_cells);
if ((x_cell > -1) && (x_cell < dimension_cells))
{
int y_cell = ty_cell + (i * dy / length_cells);
if ((y_cell > -1) && (y_cell < dimension_cells))
{
if (cell[x_cell][y_cell] == null)
cell[x_cell][y_cell] = new particleGridCellBase[dimension_cells_vertical];
for (int z_cell = 0; z_cell < height_cells; z_cell++)
{
if (cell[x_cell][y_cell][z_cell] == null)
cell[x_cell][y_cell][z_cell] = new particleGridCellBase();
float prob = 0.8f + ((float)rnd.NextDouble() * probability_variance * 2) - probability_variance;
if (prob > 0.99f) prob = 0.99f;
if (prob < 0.5f) prob = 0.5f;
particleGridCellBase c = cell[x_cell][y_cell][z_cell];
c.probabilityLogOdds = probabilities.LogOdds(prob);
c.colour = new byte[3];
c.colour[0] = (byte)r;
c.colour[1] = (byte)g;
c.colour[1] = (byte)b;
}
}
}
}
}
/// <summary>
/// inserts a simulated block into the grid
/// </summary>
/// <param name="tx_cell">start x cell coordinate</param>
/// <param name="ty_cell">start y cell coordinate</param>
/// <param name="bx_cell">end x cell coordinate</param>
/// <param name="by_cell">end y cell coordinate</param>
/// <param name="bottom_height_cells">bottom height of the block in cells</param>
/// <param name="top_height_cells">bottom height of the block in cells</param>
/// <param name="probability_variance">variation of probabilities, typically less than 0.2</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public override void InsertBlockCells(
int tx_cell, int ty_cell,
int bx_cell, int by_cell,
int bottom_height_cells,
int top_height_cells,
float probability_variance,
int r, int g, int b)
{
Random rnd = new Random(0);
if (top_height_cells >= dimension_cells_vertical)
top_height_cells = dimension_cells_vertical - 1;
if (bottom_height_cells < 0) bottom_height_cells = 0;
for (int x_cell = tx_cell; x_cell <= bx_cell; x_cell++)
{
if ((x_cell > -1) && (x_cell < dimension_cells))
{
for (int y_cell = ty_cell; y_cell <= by_cell; y_cell++)
{
if ((y_cell > -1) && (y_cell < dimension_cells))
{
if (cell[x_cell][y_cell] == null)
cell[x_cell][y_cell] = new particleGridCellBase[dimension_cells_vertical];
for (int z_cell = bottom_height_cells; z_cell <= top_height_cells; z_cell++)
{
if ((z_cell > -1) && (z_cell < dimension_cells_vertical))
{
if (cell[x_cell][y_cell][z_cell] == null)
cell[x_cell][y_cell][z_cell] = new particleGridCellBase();
float prob = 0.8f + ((float)rnd.NextDouble() * probability_variance * 2) - probability_variance;
if (prob > 0.99f) prob = 0.99f;
if (prob < 0.5f) prob = 0.5f;
particleGridCellBase c = cell[x_cell][y_cell][z_cell];
c.probabilityLogOdds = probabilities.LogOdds(prob);
c.colour = new byte[3];
c.colour[0] = (byte)r;
c.colour[1] = (byte)g;
c.colour[1] = (byte)b;
}
}
}
}
}
}
}