159 lines
3.4 KiB
Text
159 lines
3.4 KiB
Text
/*
|
|
* grid implementation
|
|
*
|
|
* Copyright (C) Uwe Hebbelmann, Sebastian Stock, Andre Schemschat
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
#include <iostream>
|
|
|
|
#include "grid/grid.h"
|
|
#include <cstdlib>
|
|
|
|
/**
|
|
* CTor.
|
|
* Allocates an array of gridPoints
|
|
* and sets the offsets and sizes
|
|
*
|
|
* @param offsetX the x-offest of the array
|
|
* @param offsetZ the z-offset of the array
|
|
* @param sizeX the x-size of the array
|
|
* @param sizeZ the y-size of the array
|
|
*/
|
|
grid::grid(long offsetX, long offsetZ, long sizeX, long sizeZ)
|
|
{
|
|
if(sizeX < 1 || sizeZ < 1)
|
|
{
|
|
std::cerr << "ERROR: Invalid array size in grid::grid! "<< std::endl;
|
|
exit(1);
|
|
}
|
|
|
|
points = NULL;
|
|
|
|
// Allocate array
|
|
this->points = new gridPoint**[sizeX];
|
|
|
|
for(int i=0; i < sizeX; ++i)
|
|
{
|
|
this->points[i] = new gridPoint*[sizeZ];
|
|
for(int j=0; j < sizeZ; ++j)
|
|
{
|
|
this->points[i][j] = new gridPoint(offsetX + i, offsetZ + j);
|
|
}
|
|
}
|
|
|
|
this->offsetX = offsetX;
|
|
this->offsetZ = offsetZ;
|
|
this->sizeX = sizeX;
|
|
this->sizeZ = sizeZ;
|
|
}
|
|
|
|
/**
|
|
* DTor. Deletes the array
|
|
*/
|
|
grid::~grid()
|
|
{
|
|
clear();
|
|
}
|
|
|
|
/**
|
|
* Frees the memory if array is set
|
|
*/
|
|
void grid::clear()
|
|
{
|
|
if(this->points == NULL)
|
|
return;
|
|
|
|
// delete all points
|
|
for(int i=0; i < getSizeX(); ++i)
|
|
{
|
|
for(int j=0; j < getSizeZ(); ++j)
|
|
{
|
|
delete this->points[i][j];
|
|
}
|
|
|
|
delete[] this->points[i];
|
|
}
|
|
|
|
delete[] this->points;
|
|
|
|
this->points = NULL;
|
|
}
|
|
|
|
/**
|
|
* Adds point to given point in the grid (updates the point)
|
|
*
|
|
* @param x the absolute x coordinate
|
|
* @param z the absolute z coordinate
|
|
* @param occupied is the point occupied or free?
|
|
* @param weight weighting of the information
|
|
*/
|
|
void grid::addPoint(long x, long z, unsigned int count, unsigned int occupied)
|
|
{
|
|
if(this->points == NULL)
|
|
{
|
|
std::cerr << "ERROR: In grid::addPoint, allocate never called!" << std::endl;
|
|
exit(1);
|
|
}
|
|
|
|
// get the absolute gridPoint
|
|
gridPoint *p = getAbsolutePoint(x, z);
|
|
|
|
// increase counters
|
|
p->addCount(count, occupied);
|
|
}
|
|
|
|
/**
|
|
* Adds new point into the grid or updates the point with new
|
|
* point information
|
|
* @param point a reference to the point (new information)
|
|
*/
|
|
void grid::addPoint(const gridPoint& point)
|
|
{
|
|
gridPoint *p = getAbsolutePoint(point.getX(), point.getZ());
|
|
p->addCount(point.getCount(), point.getOccupied());
|
|
}
|
|
|
|
/**
|
|
* Sets values for the point
|
|
*
|
|
* @param x absolute x coordinate
|
|
* @param z absolute z coordinate
|
|
* @param count new counter value (number of counts)
|
|
* @param occupied new occupied value (number of occupieds)
|
|
*/
|
|
void grid::setPoint(long x, long z, unsigned int count, unsigned int occupied)
|
|
{
|
|
gridPoint* p = getAbsolutePoint(x, z);
|
|
p->setFixed(count, occupied);
|
|
}
|
|
|
|
/**
|
|
* Returns point of the absolute coordinates
|
|
* The absolute coordinates get transformed to relative coordinates
|
|
|
|
* @param x the absolute x coordinate
|
|
* @param z the absolute z coordinate
|
|
*
|
|
* @return gridPoint* pointer to the found gridPoint
|
|
*/
|
|
gridPoint* grid::getAbsolutePoint(long x, long z) const
|
|
{
|
|
// Transform to relative coordinates
|
|
x -= getOffsetX();
|
|
z -= getOffsetZ();
|
|
|
|
if( x < 0 || x >= getSizeX() || z < 0 || z >= getSizeZ())
|
|
{
|
|
std::cerr << "ERROR: In grid::getAbsolutePoint ("
|
|
<< x << "|" << z <<") not in the grid!" << std::endl;
|
|
exit(1);
|
|
}
|
|
|
|
return this->points[x][z];
|
|
}
|
|
|
|
|
|
|