diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h index cfb19a21953b82429ba1de94b5ae1d52eee04059..5e64fd229c86dfe0b042556967a5c9fee0bc110c 100644 --- a/VirtualRobot/MathTools.h +++ b/VirtualRobot/MathTools.h @@ -153,6 +153,12 @@ namespace MathTools /************************************************************************/ struct Plane { + Plane() + { + p = Eigen::Vector3f::Zero(); + n = Eigen::Vector3f::UnitZ(); + } + Plane(const Eigen::Vector3f &point, const Eigen::Vector3f &normal) { p = point; diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp index 0426bb2c2bc30c06ebcaef6ea647c1f4a4a8d9cb..9d8d496b5fe627d520bf8286cebde4e3039640d3 100644 --- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp +++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp @@ -981,7 +981,7 @@ Eigen::Matrix4f WorkspaceRepresentation::sampleCoveredPose() { nV[j] = rand() % numVoxels[j]; } - if (data->get(nV)>0) + if (isCovered(nV)) { // create pose @@ -1026,13 +1026,13 @@ int WorkspaceRepresentation::fillHoles(unsigned int minNeighbors) for (int i=0;i<6;i++) { x[i]--; - if (data->get(x)>0) + if (isCovered(x)) { sum += data->get(x); count++; } x[i]++;x[i]++; - if (data->get(x)>0) + if (isCovered(x)) { sum += data->get(x); count++; @@ -1106,6 +1106,12 @@ bool WorkspaceRepresentation::isCovered( const Eigen::Matrix4f &globalPose ) } +bool WorkspaceRepresentation::isCovered( unsigned int v[6] ) +{ + return (data->get(v) > 0); +} + + void WorkspaceRepresentation::setVoxelEntry( unsigned int v[6], unsigned char e ) { data->setDatum(v,e); diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h index 9f29f74c3d8de6c86e7d109eb669e98eac1bddff..117960f39db579e7d8e6e8abba39203f9b5790a4 100644 --- a/VirtualRobot/Workspace/WorkspaceRepresentation.h +++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h @@ -187,10 +187,15 @@ public: Eigen::Matrix4f sampleCoveredPose(); /*! - Returns true, if the corresponding voxel entry is nun zero. + Returns true, if the corresponding voxel entry is not zero. */ bool isCovered(const Eigen::Matrix4f &globalPose); + /*! + Returns true, if voxel entry is not zero. + */ + bool isCovered(unsigned int v[6]); + virtual int getNumVoxels(int dim) const; virtual float getMinBound(int dim) const; virtual float getMaxBound(int dim) const;