void Space3D::Initialize() {
 
	//Vars for the total   space dimensions (See drawing)
	this->total_height = -(this->bottom_front_plane_height) + this->center_vert_offset + this->radius;
 
	this->total_width = 2 * this->radius;
 
	this->total_depth = this->radius + this->back_rear_plane_distance;
 
	//initialize the axis
	this->max_X = this->radius;
	this->min_X = -(this->back_rear_plane_distance);
	this->max_Y = this->radius;
	this->min_Y = -(this->radius);
	this->max_Z = this->radius + this->center_vert_offset;
	this->min_Z = this->bottom_front_plane_height;
 
	//Create and initialize the matrix
	this->create_3d_matrix();
 
}
 
/** --|Create the 3DMatrix, and initialize each cell with its center
 *
 * @return a ptr to the initialized matrix
 */
void Space3D::create_3d_matrix() {
 
	//calculate the cells needed to discretize the space, given it's dimensions and the cell size.
 
	//This could be done in a simpler way like:
	/*
	this->cells_X = (int) (this->total_depth / this->cell_size);
	this->cells_Y = (int) (this->total_width / this->cell_size);
	this->cells_Z = (int) (this->total_height / this->cell_size);
 
	But I like this way...:
	*/
 
	int pos_x_cells,neg_x_cells,pos_y_cells, neg_y_cells, pos_z_cells, neg_z_cells;
 
	//The necessary cells to cover the positive X axis
	pos_x_cells=(int)((this->max_X-(this->cell_size/2))/this->cell_size);
	//The necessary cells to cover the negative X axis
	neg_x_cells=(int)((-this->min_X-(this->cell_size/2))/this->cell_size);
	//The total number of cells counting the origin cell
	this->cells_X= pos_x_cells + neg_x_cells +1;
 
	pos_y_cells=(int)((this->max_Y-(this->cell_size/2))/this->cell_size);
	neg_y_cells=(int)((-this->min_Y-(this->cell_size/2))/this->cell_size);
	this->cells_Y= pos_y_cells + neg_y_cells +1;
 
	pos_z_cells=(int)((this->max_Z-(this->cell_size/2))/this->cell_size);
	neg_z_cells=(int)((-this->min_Z-(this->cell_size/2))/this->cell_size);
	this->cells_Z= pos_z_cells + neg_z_cells +1;
 
	//Total number of cells in the matrix;
	this->num_cells = this->cells_X * this->cells_Y * this->cells_Z;
 
	//Let's init blocked cells=0;
	this->num_blocked_cells=0;
 
	//Get memory for all the cells
	this->cellmatrix= (Cell3D**) malloc(this->num_cells*sizeof(Cell3D*));
 
	float x,y,z;
	int depth=0, col=0, row=0, total=0;
	struct accessor acc;
 
/*
	//METHOD #1 TO CONSTRUCT THE CELL MATRIX
	//TODO: Check that this is OK, and make a drawing explaining each variable
 
	for (depth=0;depth<this->cells_X;depth++){
 
		x=this->max_X - (depth * this->cell_size)- (this->cell_size/2.0);
 
		for (row=0;row<this->cells_Z;row++){
 
			z=this->max_Z - (row * this->cell_size) - (this->cell_size/2.0);
 
			for (col=0;col<this->cells_Y;col++){
 
				y=this->min_Y + (col * this->cell_size) + (this->cell_size/2.0);
 
				cellmatrix[total]=new Cell3D(x,y,z, this->cell_size);
 
				total++;
			}
		}
	}
*/
 
	//METHOD #2 TO CONSTRUCT THE MATRIX
	for( depth=0; depth<this->cells_X;depth++){
 
		x=(pos_x_cells - depth) * cell_size;
 
		for (row=0;row<this->cells_Z;row++){
 
			z=(pos_z_cells - row) * cell_size;
 
			for (col=0;col<this->cells_Y;col++){
 
				y=(-(neg_y_cells - col))*cell_size;
 
				cellmatrix[total]=new Cell3D(x,y,z,this->cell_size);
 
				//Set the accessor to the cell.
				acc.row=row;
				acc.col=col;
				acc.depth=depth;
				cellmatrix[total]->setAccessor(acc);
 
				//If this is the Origin Cell, store its accessor in the Space3D, so it can be easily accessed.
				if( (x==0.0) && (y==0.0) && (z==0.0) ){
 
					this->zero_acc.row=row;
					this->zero_acc.col=col;
					this->zero_acc.depth=depth;
				}
 
				total++;
			}
		}
 
	}
 
}
 
/** --|get a cell by its cordinates in the cellmatrix
   *
   *
   * @param row Matrix Accessor
   * @param col Matrix Accessor
   * @param depth Matrix Accessor
   *
   * @return A ptr to the cell, or null if it does not exist.
   */
Cell3D* Space3D::getCell3D_by_accessor(int row, int col, int depth ){
 
	int index;
 
	index=(depth*(this->cells_Z*cells_Y) + row*this->cells_Y + col);
 
	if(index < this->num_cells){
		return this->cellmatrix[index];
	}else{
		return NULL;
	}
}
 
/** --|Return a cell by its coordinates in 3D Space
   *
   *
   * @param x 3D Coordinate
   * @param y 3D Coordinate
   * @param z 3D Coordinate
   *
   * @return A ptr to the cell, or NULL if it does not exist.
   */
Cell3D* Space3D::getCell3D_by_coord(float x, float y, float z){
 
	int row, col, depth;
	int q,r;
 
	//Coords out of range
	if( (x>=this->max_X) || (x<this->min_X) || (y>=this->max_Y) || (y<this->min_Y) || (z>=this->max_Z) || (z<this->min_Z)  ) return NULL;
 
	if(x>=0){
		q=(int)(x/this->cell_size);
		r=((int)x%(int)this->cell_size);
 
		if( r>=(int)(this->cell_size/2.0) ){
			depth=this->zero_acc.depth-q-1;
		}else{
			depth=this->zero_acc.depth-q;
		}
 
	}else{
		q=(int)(fabs(x)/this->cell_size);
		r=((int)fabs(x)%(int)this->cell_size);
 
		if( r<=(int)(this->cell_size/2.0) ){
			depth=this->zero_acc.depth+q;
		}else{
			depth=this->zero_acc.depth+q+1;
		}
	}
 
	if(y>=0){
		q=(int)(y/this->cell_size);
		r=((int)y%(int)this->cell_size);
 
		if( r>=(int)(this->cell_size/2.0) ){
			col=this->zero_acc.col+q+1;
		}else{
			col=this->zero_acc.col+q;
		}
 
	}else{
		q=(int)(fabs(y)/this->cell_size);
		r=((int)fabs(y)%(int)this->cell_size);
 
		if( r<=(int)(this->cell_size/2.0) ){
			col=this->zero_acc.col-q;
		}else{
			col=this->zero_acc.col-q-1;
		}
	}
 
	if(z>=0){
		q=(int)(z/this->cell_size);
		r=((int)z%(int)this->cell_size);
 
		if( r>=(int)(this->cell_size/2.0) ){
			row=this->zero_acc.row+q+1;
		}else{
			row=this->zero_acc.row+q;
		}
 
	}else{
		q=(int)(fabs(z)/this->cell_size);
		r=((int)fabs(z)%(int)this->cell_size);
 
		if( r<=(int)(this->cell_size/2.0) ){
			row=this->zero_acc.row-q;
		}else{
			row=this->zero_acc.row-q-1;
		}
	}
 
	return this->getCell3D_by_accessor(row, col, depth);
 
}