Пример #1
0
		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);
            
		}
Пример #2
0
        /// <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);
                }
            }
        }
Пример #3
0
        /// <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++;
                        }
                    }
                }
            }
        }
Пример #5
0
 /// <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;
 }
Пример #6
0
        /// <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);
            }
        }
Пример #7
0
        /// <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;
		}
Пример #8
0
        /// <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);
        }
Пример #9
0
		/// <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;
						}
					}
				}				
			}
		}		
Пример #10
0
		/// <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;
						}
					}
				}				
			}
		}
Пример #11
0
		/// <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;
								}
							}
						}
					}
				}
			}
		}