Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- *******************************************************************************
- PointCloudNav.h
- *******************************************************************************
- */
- #pragma once
- #include <iostream>
- #include <cstdlib>
- #include <list>
- #include <sstream>
- #include <iomanip>
- using std::cerr;
- using std::cout;
- using std::endl;
- using std::rand;
- using std::list;
- using std::allocator;
- using std::string;
- using std::stringstream;
- using std::ostream;
- namespace pcn
- {
- int ERROR_CODE_INIT_VOXEL_POSITION = 1;
- int ERROR_CODE_INIT_VOXEL_DENSITY = 2;
- int ERROR_CODE_INIT_OCTREE_VOXELS = 3;
- int ERROR_CODE_INVALID_LOWER_LIMIT = 4;
- int ERROR_CODE_INVALIT_UPPER_LIMIT = 5;
- class PCVoxel
- {
- private:
- double position[3];
- double density;
- void init(const double x, const double y, const double z, const double d)
- {
- // cout << x << ", " << y << ", " << z << endl;
- if (d < 0.0 || d > 1.0)
- {
- cerr << "Incorrect value for density of Voxel";
- exit(ERROR_CODE_INIT_VOXEL_DENSITY);
- }
- position[0] = x;
- position[1] = y;
- position[2] = z;
- density = d;
- }
- public:
- PCVoxel()
- {
- init(0.0, 0.0, 0.0, 1.0);
- }
- PCVoxel(const double x, const double y, const double z)
- {
- readDensity();
- init(x, y, z, this->getDensity());
- //cout << this->getXPosition() << ", " << this->getYPosition() << ", " << this->getZPosition() << endl;
- }
- PCVoxel(const double x, const double y, const double z, const double d)
- {
- init(x, y, z, d);
- }
- PCVoxel(double pos[], double d)
- {
- if (d < 0.0 || d > 1.0)
- {
- cerr << "Incorrect array size for position of Voxel";
- exit(ERROR_CODE_INIT_VOXEL_POSITION);
- }
- init(pos[0], pos[1], pos[2], d);
- }
- PCVoxel(PCVoxel* rhs)
- {
- init((*rhs).position[0], (*rhs).position[1],
- (*rhs).position[2], (*rhs).density);
- }
- ~PCVoxel() { delete [] position; }
- PCVoxel& operator=(PCVoxel const& rhs);
- bool operator==(PCVoxel const& rhs);
- bool operator<(const PCVoxel& rhs);
- void setXPosition (const double x) { position[0] = x; }
- void setYPosition (const double y) { position[1] = y; }
- void setZPosition (const double z) { position[2] = z; }
- void setDensity (const double d) { density = d; }
- double getXPosition() const { return position[0]; }
- double getYPosition() const { return position[1]; }
- double getZPosition() const { return position[2]; }
- double getDensity() const { return density; }
- void readDensity() {
- // TO DO: implement proper method for reading the density of the voxel
- density = (double)(rand() % 100) / 100;
- }
- inline string getObjectAsString() const;
- static PCVoxel* interpolate(const PCVoxel& first, const PCVoxel& second);
- };
- inline ostream& operator<<(ostream& lhs, const PCVoxel& rhs)
- {
- lhs << rhs.getObjectAsString();
- return lhs;
- }
- typedef list<PCVoxel, allocator<PCVoxel> > LISTVOXELS;
- typedef PCVoxel ARRAYVOXELS[8];
- class PCOctree
- {
- private:
- PCVoxel **borderPoints;
- double repel;
- public:
- PCOctree();
- PCOctree(ARRAYVOXELS &voxels);
- ~PCOctree() { delete [] borderPoints; }
- PCVoxel** getBorderPoints() const;
- double getRepel() const;
- PCOctree* breakDown () const;
- };
- ostream& operator<<(ostream& lhs, const PCOctree& rhs)
- {
- stringstream toString;
- PCVoxel **borderPoints = rhs.getBorderPoints();
- for (int i = 0; i < 8; i++)
- {
- cout << *(borderPoints[i]) << endl;
- toString << *(borderPoints[i]) << endl;
- }
- return lhs << toString;
- }
- class PCGrid
- {
- friend std::ostream& operator<<(std::ostream& lhs, const PCGrid& rhs);
- private:
- int sizeX, sizeY, sizeZ;
- PCVoxel **grid;
- void init(const double xMin, const double yMin, const double zMin,
- const double xMax, const double yMax, const double zMax,
- const int divX, const int divY, const int divZ);
- public:
- PCGrid();
- PCGrid(const double xMin, const double yMin, const double zMin,
- const double xMax, const double yMax, const double zMax,
- const int divX, const int divY, const int divZ);
- ~PCGrid() { delete [] grid; }
- int getSizeOfX() const;
- int getSizeOfY() const;
- int getSizeOfZ() const;
- PCVoxel& getVoxelAt(int posX, int posY, int posZ) const;
- double* readDensities();
- };
- ostream& operator<<(ostream& lhs, const PCGrid& rhs)
- {
- stringstream toString;
- int sizeX = rhs.getSizeOfX(), sizeY = rhs.getSizeOfY(),
- sizeZ = rhs.getSizeOfZ();
- PCVoxel current;
- for (int x = 0; x < sizeX; x++)
- {
- for (int y = 0; y < sizeY; y++)
- {
- for (int z = 0; z < sizeZ; z++)
- {
- toString << rhs.getVoxelAt(x, y, z) << endl;
- }
- }
- }
- return lhs << toString;
- }
- class PointCloudNavigation
- {
- private:
- PCGrid *grid;
- LISTVOXELS getSurface( const PCOctree &oct, double rMin, double rMax );
- public:
- PointCloudNavigation(
- double xMin, double yMin, double zMin,
- double xMax, double yMax, double zMax,
- int divX, int divY, int divZ);
- ~PointCloudNavigation() { delete grid; }
- PCGrid* accessGrid ();
- LISTVOXELS findPathMesh( double tolFloor, double tolCeiling );
- };
- };
- /*
- *******************************************************************************
- PointCloudNav.cpp
- *******************************************************************************
- */
- #include <iostream>
- #include <cstdlib>
- #include "PointCloudNav.h"
- using namespace pcn;
- using std::cerr;
- using std::cout;
- using std::cin;
- using std::endl;
- using std::rand;
- /*
- *******************************************************************************
- VOXEL
- *******************************************************************************
- */
- PCVoxel& PCVoxel::operator=(const PCVoxel& rhs)
- {
- delete [] position;
- init(rhs.position[0],rhs.position[1],
- rhs.position[2], rhs.density);
- return *this;
- }
- bool PCVoxel::operator==(const PCVoxel& rhs)
- {
- bool equalX = this->getXPosition() == rhs.getXPosition(),
- equalY = this->getYPosition() == rhs.getYPosition(),
- equalZ = this->getZPosition() == rhs.getZPosition(),
- equalD = this->getDensity() == rhs.getDensity();
- return equalX && equalY && equalZ && equalD;
- }
- bool PCVoxel::operator<(const PCVoxel& rhs)
- {
- if (this->getXPosition() == rhs.getXPosition())
- {
- if (this->getYPosition() == rhs.getYPosition())
- {
- if (this->getZPosition() < rhs.getZPosition())
- {
- return true;
- }
- else { return false; }
- }
- else if (this->getYPosition() < rhs.getYPosition())
- {
- return true;
- }
- else { return false; }
- }
- else if (this->getXPosition() < rhs.getXPosition())
- {
- return true;
- }
- else
- {
- return false;
- }
- }
- PCVoxel* PCVoxel::interpolate(const PCVoxel& first, const PCVoxel& second)
- {
- double meanX, meanY, meanZ, meanD;
- meanX = (first.getXPosition() + second.getXPosition()) / 2;
- meanY = (first.getYPosition() + second.getYPosition()) / 2;
- meanZ = (first.getZPosition() + second.getZPosition()) / 2;
- meanD = ((double)(rand() % 100) / 100) * (second.getDensity() - first.getDensity()) + first.getDensity();
- //cout << "Density: " << meanD << endl;
- return new PCVoxel(meanX, meanY, meanZ, meanD);
- }
- string PCVoxel::getObjectAsString() const
- {
- stringstream toString;
- toString << "( " << position[0] << ", " << position[1] << ", "
- << position[2] << ") \ndensity of point cloud: " << getDensity() << endl;
- return toString.str();
- }
- /*
- *******************************************************************************
- OCTREE
- *******************************************************************************
- */
- PCOctree::PCOctree() {}
- PCOctree::PCOctree(ARRAYVOXELS &voxels)
- {
- cout << "DEBUG: size of voxels: " << sizeof(voxels) / sizeof(PCVoxel) << endl;
- if ((sizeof(voxels) / sizeof(PCVoxel)) != 8)
- {
- cerr << "Invalid number of Voxels given" << endl;
- exit(ERROR_CODE_INIT_OCTREE_VOXELS);
- }
- else
- {
- borderPoints = new PCVoxel*[8];
- for (int i = 0; i < 8; i++)
- borderPoints[i] = &(voxels[i]);
- }
- }
- PCVoxel** PCOctree::getBorderPoints() const
- {
- return borderPoints;
- }
- double PCOctree::getRepel () const
- {
- double densityTotal = 0;
- PCVoxel** borderPoints = getBorderPoints();
- for ( int pos = 0; pos < 8; pos++)
- {
- PCVoxel* current = borderPoints[pos];
- cout << *current << endl;
- densityTotal += current->getDensity();
- //cout << "cumulative total: " << densityTotal << endl;
- }
- return densityTotal / 8;
- }
- PCOctree* PCOctree::breakDown () const
- {
- PCVoxel *helper = new PCVoxel[19];
- const int H01 = 0, H02 = 1, H04 = 2, H13 = 3, H15 = 4, H23 = 5,
- H26 = 6, H37 = 7, H45 = 8, H46 = 9, H57 = 10, H67 = 11,
- H_TOP = 12, H_BOTTOM = 13, H_SOUTH = 14, H_NORTH = 15,
- H_EAST = 16, H_WEST = 17, H_CENTER = 18;
- helper[H01] = PCVoxel::interpolate(*borderPoints[0], *borderPoints[1]);
- helper[H02] = PCVoxel::interpolate(*borderPoints[0], *borderPoints[2]);
- helper[H04] = PCVoxel::interpolate(*borderPoints[0], *borderPoints[4]);
- helper[H13] = PCVoxel::interpolate(*borderPoints[1], *borderPoints[3]);
- helper[H15] = PCVoxel::interpolate(*borderPoints[1], *borderPoints[5]);
- helper[H23] = PCVoxel::interpolate(*borderPoints[2], *borderPoints[3]);
- helper[H26] = PCVoxel::interpolate(*borderPoints[2], *borderPoints[6]);
- helper[H37] = PCVoxel::interpolate(*borderPoints[3], *borderPoints[7]);
- helper[H45] = PCVoxel::interpolate(*borderPoints[4], *borderPoints[5]);
- helper[H46] = PCVoxel::interpolate(*borderPoints[4], *borderPoints[6]);
- helper[H57] = PCVoxel::interpolate(*borderPoints[5], *borderPoints[7]);
- helper[H67] = PCVoxel::interpolate(*borderPoints[6], *borderPoints[7]);
- helper[H_BOTTOM] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H01], helper[H45]),
- *PCVoxel::interpolate(helper[H04], helper[H15]));
- helper[H_TOP] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H23], helper[H67]),
- *PCVoxel::interpolate(helper[H26], helper[H37]));
- helper[H_NORTH] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H13], helper[H57]),
- *PCVoxel::interpolate(helper[H15], helper[H37]));
- helper[H_SOUTH] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H02], helper[H46]),
- *PCVoxel::interpolate(helper[H04], helper[H26]));
- helper[H_WEST] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H02], helper[H13]),
- *PCVoxel::interpolate(helper[H01], helper[H23]));
- helper[H_EAST] = PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H46], helper[H57]),
- *PCVoxel::interpolate(helper[H45], helper[H67]));
- helper[H_CENTER] = PCVoxel::interpolate(
- *PCVoxel::interpolate(
- *PCVoxel::interpolate(helper[H_TOP], helper[H_BOTTOM]),
- *PCVoxel::interpolate(helper[H_SOUTH], helper[H_NORTH])
- ), *PCVoxel::interpolate(helper[H_EAST], helper[H_WEST]));
- /*
- 3-----37-----7
- /| /|
- 23 | 67 |
- / 13 / 57
- 2---+-26-----6 |
- | | | |
- | 1-----15-+---5
- 02 / 46 /
- | 01 | 45 y z
- |/ |/ |/
- 0-----04-----4 0--x
- */
- PCVoxel relevant[8][8] = {
- // 1st Subtree
- { borderPoints[0], helper[H01], helper[H02], helper[H_WEST],
- helper[H04], helper[H_BOTTOM], helper[H_SOUTH], helper[H_CENTER] },
- // 2nd Subtree
- { helper[H01], borderPoints[1], helper[H_WEST], helper[H13],
- helper[H_BOTTOM], helper[H15], helper[H_CENTER], helper[H_NORTH] },
- // 3rd Subtree
- { helper[H02], helper[H_WEST], borderPoints[2], helper[H23],
- helper[H_SOUTH], helper[H_CENTER], helper[H26], helper[H_TOP] },
- // 4th Subtree
- { helper[H_WEST], helper[H13], helper[H23], borderPoints[3],
- helper[H_CENTER], helper[H_NORTH], helper[H_TOP], helper[H37] },
- // 5th Subtree
- { helper[H04], helper[H_BOTTOM], helper[H_SOUTH], helper[H_CENTER],
- borderPoints[4], helper[H45], helper[H46], helper[H_EAST] },
- // 6th Subtree
- { helper[H_BOTTOM], helper[H15], helper[H_CENTER], helper[H_NORTH],
- helper[H45], borderPoints[5], helper[H_EAST], helper[H57] },
- // 7th Subtree
- { helper[H_SOUTH], helper[H_CENTER], helper[H26], helper[H_TOP],
- helper[H46], helper[H_EAST], borderPoints[6], helper[H67] },
- // 8th Subtree
- { helper[H_CENTER], helper[H_NORTH], helper[H_TOP], helper[H37],
- helper[H_EAST], helper[H57], helper[H67], borderPoints[7] },
- };
- PCOctree *solution_depot[8];
- for (int i = 0; i < 8; i++)
- solution_depot[i] = new PCOctree(relevant[i]);
- PCOctree solution[8] = { *solution_depot[0], *solution_depot[1],
- *solution_depot[2], *solution_depot[3], *solution_depot[4],
- *solution_depot[5], *solution_depot[6], *solution_depot[7] };
- return solution;
- }
- /*
- *******************************************************************************
- GRID
- *******************************************************************************
- */
- void PCGrid::init(const double xMin, const double yMin, const double zMin,
- const double xMax, const double yMax, const double zMax,
- const int divX, const int divY, const int divZ)
- {
- int index = 0;
- double xStep = (xMax - xMin)/divX, yStep = (yMax - yMin)/divY,
- zStep = (zMax - zMin)/divZ;
- grid = new PCVoxel *[sizeX * sizeY * sizeZ];
- for (double posX = xMin; posX <= xMax; posX += xStep)
- {
- for (double posY = yMin; posY <= yMax; posY += yStep)
- {
- for (double posZ = zMin; posZ <= zMax; posZ += zStep)
- {
- grid[index] = new PCVoxel(posX, posY, posZ);
- //cout << *(grid[index]) << endl;
- index++;
- }
- }
- }
- }
- PCGrid::PCGrid(const double xMin, const double yMin, const double zMin,
- const double xMax, const double yMax, const double zMax,
- const int divX, const int divY, const int divZ)
- {
- this->sizeX = divX + 1;
- this->sizeY = divY + 1;
- this->sizeZ = divZ + 1;
- init(xMin, yMin, zMin, xMax, yMax, zMax, divX, divY, divZ);
- }
- int PCGrid::getSizeOfX() const { return sizeX; }
- int PCGrid::getSizeOfY() const { return sizeY; }
- int PCGrid::getSizeOfZ() const { return sizeZ; }
- PCVoxel& PCGrid::getVoxelAt(int posX, int posY, int posZ) const
- {
- return *grid[posZ + posY * sizeY + posX * sizeX];
- }
- /*
- *******************************************************************************
- POINT CLOUD NAVIGATION
- *******************************************************************************
- */
- PointCloudNavigation::PointCloudNavigation(
- double xMin, double yMin, double zMin,
- double xMax, double yMax, double zMax,
- int divX, int divY, int divZ)
- {
- grid = new PCGrid( xMin, yMin, zMin,
- xMax, yMax, zMax,
- divX, divY, divZ);
- }
- PCGrid* PointCloudNavigation::accessGrid ()
- {
- return grid;
- }
- LISTVOXELS PointCloudNavigation::findPathMesh( double tolFloor, double tolCeiling )
- {
- if ( tolFloor < 0.0 || tolFloor > 1.0 )
- {
- cerr << "Invalid Value for lower tolerance level" << endl;
- exit(ERROR_CODE_INVALID_LOWER_LIMIT);
- }
- if ( tolCeiling < 0.0 || tolCeiling > 1.0 )
- {
- cerr << "Invalid Value for upper tolerance level" << endl;
- exit(ERROR_CODE_INVALIT_UPPER_LIMIT);
- }
- LISTVOXELS solution;
- for (int posX = 1; posX < (*grid).getSizeOfX(); posX++)
- {
- for (int posY = 1; posY < (*grid).getSizeOfY(); posY++)
- {
- for (int posZ = 1; posZ < (*grid).getSizeOfZ(); posZ++)
- {
- PCVoxel relevant[8] = {
- (*grid).getVoxelAt(posX-1, posY-1, posZ-1),
- (*grid).getVoxelAt(posX-1, posY-1, posZ ),
- (*grid).getVoxelAt(posX-1, posY , posZ-1),
- (*grid).getVoxelAt(posX-1, posY , posZ ),
- (*grid).getVoxelAt(posX , posY-1, posZ-1),
- (*grid).getVoxelAt(posX , posY-1, posZ ),
- (*grid).getVoxelAt(posX , posY , posZ-1),
- (*grid).getVoxelAt(posX , posY , posZ )
- };
- // cout << "DEBUG: size of voxels: " << sizeof(relevant) / sizeof(PCVoxel*) << endl;
- PCOctree examine(relevant);
- cout << "\nImpending insertion:" << endl;
- cout << "size of solution set before: " << solution.size() << endl;
- LISTVOXELS tmp_solution = getSurface(examine, tolFloor, tolCeiling);
- if (tmp_solution.empty())
- {
- cout << "no Voxels added to solution" << endl;
- continue;
- }
- solution.merge(tmp_solution);
- solution.unique();
- cout << "size of soulution set after: " << solution.size() << endl;
- delete [] relevant;
- }
- }
- }
- return solution;
- }
- LISTVOXELS PointCloudNavigation::getSurface( const PCOctree &oct, double rMin, double rMax )
- {
- LISTVOXELS solution;
- //PCOctree v_oct = oct;
- list<PCVoxel>::iterator iter;
- iter = solution.begin();
- double repel = oct.getRepel(); // v_oct.getRepel();
- //cout << "\nRepel of Octree: " << repel << endl;
- if ( repel > rMax )
- {
- PCVoxel** borderPoints = oct.getBorderPoints(); // v_oct.getBorderPoints();
- for (int i = 0; i < 8; i++)
- {
- cout << "Punkt: " << *borderPoints[i];
- solution.insert(iter, *borderPoints[i]);
- ++iter;
- }
- }
- else if ( repel > rMin && repel < rMax)
- {
- PCOctree *breakDown = oct.breakDown(); // v_oct.breakDown();
- for (int i = 0; i < 8; i++)
- {
- PCOctree current;
- cout << current << endl;
- LISTVOXELS tmp_solution = getSurface(current, rMin, rMax);
- if (!tmp_solution.empty())
- solution.merge(tmp_solution);
- }
- }
- return solution;
- }
- int main(int argc, int argv[])
- {
- cout << "Building Grid..." << endl; // << pcn->accessGrid() << endl;
- PointCloudNavigation pcn(0, 0, 0, 100, 100, 10, 100, 100, 10);
- cout << "Grid built!" << endl;
- cout << endl << "Processing solution..." << endl;
- LISTVOXELS solution = pcn.findPathMesh(0.40, 0.55);
- LISTVOXELS::const_iterator iter;
- for (iter = solution.begin(); iter != solution.end(); iter++)
- cout << "Lösung: " << endl << *iter << endl;
- char c;
- cin >> c;
- return 0;
- }
Add Comment
Please, Sign In to add comment