From 3117571d047a5480988a46aa6edcb031ac5ee27a Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Fri, 19 Jul 2013 14:47:24 +0000
Subject: [PATCH] minor helper methods
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@401 042f3d55-54a8-47e9-b7fb-15903f145c44
---
VirtualRobot/MathTools.h | 6 ++++++
VirtualRobot/Workspace/WorkspaceRepresentation.cpp | 12 +++++++++---
VirtualRobot/Workspace/WorkspaceRepresentation.h | 7 ++++++-
3 files changed, 21 insertions(+), 4 deletions(-)
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index cfb19a219..5e64fd229 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 0426bb2c2..9d8d496b5 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 9f29f74c3..117960f39 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;
--
GitLab