diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 7b47882833332509d7c428f3d53b2749e72e8089..0963d2dcc7a0d37cc27fa07baa0652d0f805295a 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -68,7 +68,7 @@ IK/PoseQualityExtendedManipulability.cpp
IK/HierarchicalIK.cpp
IK/JointLimitAvoidanceJacobi.cpp
IK/GazeIK.cpp
-Workspace/WorkspaceData.cpp
+Workspace/WorkspaceDataArray.cpp
Workspace/WorkspaceRepresentation.cpp
Workspace/Reachability.cpp
Workspace/Manipulability.cpp
@@ -148,6 +148,7 @@ IK/HierarchicalIK.h
IK/JointLimitAvoidanceJacobi.h
IK/GazeIK.h
Workspace/WorkspaceData.h
+Workspace/WorkspaceDataArray.h
Workspace/WorkspaceRepresentation.h
Workspace/Reachability.h
Workspace/Manipulability.h
diff --git a/VirtualRobot/VirtualRobot.h b/VirtualRobot/VirtualRobot.h
index b4948f36b06e2eaaa8a8a8c548b9bf443b99b7a8..43ab2b83ea7918ee1941e1176bc2167b7396d49b 100644
--- a/VirtualRobot/VirtualRobot.h
+++ b/VirtualRobot/VirtualRobot.h
@@ -176,6 +176,7 @@ namespace VirtualRobot
class SphereApproximator;
class BasicGraspQualityMeasure;
class WorkspaceGrid;
+ class WorkspaceDataArray;
typedef boost::shared_ptr<RobotNode> RobotNodePtr;
typedef boost::shared_ptr<RobotNodeSet> RobotNodeSetPtr;
@@ -197,6 +198,7 @@ namespace VirtualRobot
typedef boost::shared_ptr<VisualizationNode> VisualizationNodePtr;
typedef boost::shared_ptr<VisualizationFactory> VisualizationFactoryPtr;
typedef boost::shared_ptr<WorkspaceData> WorkspaceDataPtr;
+ typedef boost::shared_ptr<WorkspaceDataArray> WorkspaceDataArrayPtr;
typedef boost::shared_ptr<WorkspaceRepresentation> WorkspaceRepresentationPtr;
typedef boost::shared_ptr<Reachability> ReachabilityPtr;
typedef boost::shared_ptr<Scene> ScenePtr;
diff --git a/VirtualRobot/Workspace/Manipulability.cpp b/VirtualRobot/Workspace/Manipulability.cpp
index 886514a3c1f47e73543a0da871d6a4d7698ab057..3755b869b47507df1b273d28aaa0b3a639dff6cb 100644
--- a/VirtualRobot/Workspace/Manipulability.cpp
+++ b/VirtualRobot/Workspace/Manipulability.cpp
@@ -459,7 +459,7 @@ bool Manipulability::smooth(unsigned int minNeighbors)
minNeighbors = 1;
// copy data
- WorkspaceDataPtr newData(new WorkspaceData(data));
+ WorkspaceDataPtr newData(data->clone());
int s = 1;
for (int a=s;a<(int)data->getSize(0)-s;a++)
@@ -575,7 +575,7 @@ VirtualRobot::WorkspaceRepresentationPtr Manipulability::clone()
memcpy(res->spaceSize,this->spaceSize,sizeof(float)*6);
res->adjustOnOverflow = this->adjustOnOverflow;
- res->data.reset(new WorkspaceData(this->data));
+ res->data.reset(this->data->clone());
res->measure = this->measure;
res->maxManip = this->maxManip;
diff --git a/VirtualRobot/Workspace/Reachability.cpp b/VirtualRobot/Workspace/Reachability.cpp
index 51cf9e8503375c717466aeaa9f329cff013963c5..58623af0df9e3017b4a8f2c1fdd67a14568f6864 100644
--- a/VirtualRobot/Workspace/Reachability.cpp
+++ b/VirtualRobot/Workspace/Reachability.cpp
@@ -99,7 +99,7 @@ VirtualRobot::WorkspaceRepresentationPtr Reachability::clone()
memcpy(res->spaceSize,this->spaceSize,sizeof(float)*6);
res->adjustOnOverflow = this->adjustOnOverflow;
- res->data.reset(new WorkspaceData(this->data));
+ res->data.reset(this->data->clone());
return res;
}
diff --git a/VirtualRobot/Workspace/WorkspaceData.h b/VirtualRobot/Workspace/WorkspaceData.h
index b179328419863ffffd5128f3dc4c9d55afa3cd55..fbab964174162eac99969284e4dd18afe494ff02 100644
--- a/VirtualRobot/Workspace/WorkspaceData.h
+++ b/VirtualRobot/Workspace/WorkspaceData.h
@@ -1,13 +1,13 @@
/**
* This file is part of Simox.
*
-* Simox is free software; you can redistribute it and/or modify
+* Simox is free software = 0; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
-* published by the Free Software Foundation; either version 2 of
+* published by the Free Software Foundation = 0; either version 2 of
* the License, or (at your option) any later version.
*
* Simox is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
+* WITHOUT ANY WARRANTY = 0; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
@@ -43,161 +43,67 @@ namespace VirtualRobot
*/
class VIRTUAL_ROBOT_IMPORT_EXPORT WorkspaceData : public boost::enable_shared_from_this<WorkspaceData>
{
-public:
- /*!
- Constructor, fills the data with 0
- */
- WorkspaceData(unsigned int size1, unsigned int size2, unsigned int size3,
- unsigned int size4, unsigned int size5, unsigned int size6, bool adjustOnOverflow);
-
- //! Clone other data structure
- WorkspaceData(WorkspaceDataPtr other);
-
- ~WorkspaceData();
+public:
//! Return the amount of data in bytes
- unsigned int getSizeTr() const;
- unsigned int getSizeRot() const;
+ virtual unsigned int getSizeTr() const = 0;
+ virtual unsigned int getSizeRot() const = 0;
- inline void getPos( unsigned int x0, unsigned int x1, unsigned int x2,
+ virtual void getPos( unsigned int x0, unsigned int x1, unsigned int x2,
unsigned int x3, unsigned int x4, unsigned int x5 ,
- unsigned int &storePosTr, unsigned int &storePosRot) const
- {
- storePosTr = x0 * sizeTr0 + x1 * sizeTr1 + x2;
- storePosRot = x3 * sizeRot0 + x4 * sizeRot1 + x5;
- }
-
- inline void getPos( unsigned int x[6], unsigned int &storePosTr, unsigned int &storePosRot ) const
- {
- storePosTr = x[0] * sizeTr0 + x[1] * sizeTr1 + x[2];
- storePosRot = x[3] * sizeRot0 + x[4] * sizeRot1 + x[5];
- }
-
- inline void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value)
- {
- ensureData(x0,x1,x2);
- unsigned int posTr = 0, posRot = 0;
- getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
- if (data[posTr][posRot]==0)
- voxelFilledCount++;
- data[posTr][posRot] = value;
- if (value >= maxEntry)
- maxEntry = value;
- }
-
- inline void setDatum(unsigned int x[6], unsigned char value)
- {
- ensureData(x[0],x[1],x[2]);
- unsigned int posTr = 0, posRot = 0;
- getPos(x,posTr,posRot);
- if (data[posTr][posRot]==0)
- voxelFilledCount++;
- data[posTr][posRot] = value;
- if (value >= maxEntry)
- maxEntry = value;
- }
-
- void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels);
-
- inline void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5)
- {
- ensureData(x0,x1,x2);
- unsigned int posTr = 0, posRot = 0;
- getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
- unsigned char e = data[posTr][posRot];
- if (e==0)
- voxelFilledCount++;
- if (e<UCHAR_MAX)
- {
- data[posTr][posRot]++;
- if (e >= maxEntry)
- maxEntry = e+1;
- } else if (adjustOnOverflow)
- bisectData();
- }
- inline void increaseDatum( unsigned int x[6] )
- {
- ensureData(x[0],x[1],x[2]);
- unsigned int posTr = 0, posRot = 0;
- getPos(x,posTr,posRot);
- unsigned char e = data[posTr][posRot];
- if (e==0)
- voxelFilledCount++;
- if (e<UCHAR_MAX)
- {
- data[posTr][posRot]++;
- if (e >= maxEntry)
- maxEntry = e+1;
- } else if (adjustOnOverflow)
- bisectData();
- }
+ unsigned int &storePosTr, unsigned int &storePosRot) const = 0;
+
+ virtual void getPos( unsigned int x[6], unsigned int &storePosTr, unsigned int &storePosRot ) const = 0;
+
+ virtual void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value) = 0;
+
+ virtual void setDatum(unsigned int x[6], unsigned char value) = 0;
+
+ virtual void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels) = 0;
+
+ virtual void increaseDatum( unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5) = 0;
+
+ virtual void increaseDatum( unsigned int x[6] ) = 0;
/*!
Set rotation data for given x,y,z position.
*/
- void setDataRot(unsigned char *data, unsigned int x, unsigned int y, unsigned int z);
+ virtual void setDataRot(unsigned char *data, unsigned int x, unsigned int y, unsigned int z) = 0;
/*!
Get rotation data for given x,y,z position.
*/
- const unsigned char *getDataRot(unsigned int x, unsigned int y, unsigned int z);
+ virtual const unsigned char *getDataRot(unsigned int x, unsigned int y, unsigned int z) = 0;
//! Simulates a multi-dimensional array access
- inline unsigned char get(unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5) const
- {
- unsigned int posTr = 0, posRot = 0;
- getPos(x0,x1,x2,x3,x4,x5,posTr,posRot);
- if (data[posTr])
- return data[posTr][posRot];
- else
- return 0;
- }
+ virtual unsigned char get(unsigned int x0, unsigned int x1, unsigned int x2,
+ unsigned int x3, unsigned int x4, unsigned int x5) const = 0;
//! Simulates a multi-dimensional array access
- inline unsigned char get( unsigned int x[6] ) const
- {
- unsigned int posTr = 0, posRot = 0;
- getPos(x,posTr,posRot);
- if (data[posTr])
- return data[posTr][posRot];
- else
- return 0;
- }
-
- bool hasEntry(unsigned int x, unsigned int y, unsigned int z);
+ virtual unsigned char get( unsigned int x[6] ) const = 0;
+
+ virtual bool hasEntry(unsigned int x, unsigned int y, unsigned int z) = 0;
// Set all entries to 0
- void clear();
+ virtual void clear() = 0;
- unsigned char getMaxEntry() const;
- unsigned int getVoxelFilledCount() const;
- void binarize();
+ virtual unsigned char getMaxEntry() const = 0;
+ virtual unsigned int getVoxelFilledCount() const = 0;
+ virtual void binarize() = 0;
- void bisectData();
- void ensureData(unsigned int x, unsigned int y, unsigned int z);
+ virtual void bisectData() = 0;
- void setVoxelFilledCount(int c){voxelFilledCount = c;}
- void setMaxEntry(unsigned char m){maxEntry = m;}
+ virtual void setVoxelFilledCount(int c) = 0;
+ virtual void setMaxEntry(unsigned char m) = 0;
- unsigned int getSize(int dim){return sizes[dim];}
+ virtual unsigned int getSize(int dim) = 0;
//! Min valid value is 1 by default. In cases some values are needed to indicate special flags (e.g. stability) the minimum valid number can be set here
- void setMinValidValue(unsigned char v);
-
- unsigned char** getRawData(){return data;}
-protected:
- unsigned int sizes[6];
- unsigned int sizeTr0,sizeTr1;
- unsigned int sizeRot0,sizeRot1;
-
- unsigned char** data;
+ virtual void setMinValidValue(unsigned char v) = 0;
- unsigned char minValidValue;
+ virtual unsigned char** getRawData() = 0;
- unsigned char maxEntry;
- unsigned int voxelFilledCount;
- bool adjustOnOverflow;
+ virtual WorkspaceData* clone() = 0;
};
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index 521099d298f9a79cb9018cf9365f31657e3b4681..7d9cf3ddb907b93d21323cb9e3aead6235748c64 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -267,7 +267,7 @@ void WorkspaceRepresentation::load(const std::string &filename)
THROW_VR_EXCEPTION_IF(tmpString != "DATA_START", "Bad file format, expecting DATA_START.");
long size = numVoxels[0]*numVoxels[1]*numVoxels[2]*numVoxels[3]*numVoxels[4]*numVoxels[5];
- data.reset(new WorkspaceData(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5],true));
+ data.reset(new WorkspaceDataArray(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5],true));
if (version[0]<=1 || (version[0]==2 && version[1]<=3))
{
@@ -923,7 +923,7 @@ void WorkspaceRepresentation::initialize( RobotNodeSetPtr nodeSet, float discret
THROW_VR_EXCEPTION_IF( (numVoxels[i]<=0), " numVoxels <= 0 in dimension " << i);
}
- data.reset(new WorkspaceData(numVoxels[0],numVoxels[1],numVoxels[2],numVoxels[3],numVoxels[4],numVoxels[5],adjustOnOverflow));
+ data.reset(new WorkspaceDataArray(numVoxels[0],numVoxels[1],numVoxels[2],numVoxels[3],numVoxels[4],numVoxels[5],adjustOnOverflow));
}
void WorkspaceRepresentation::binarize()
@@ -1015,7 +1015,7 @@ Eigen::Matrix4f WorkspaceRepresentation::sampleCoveredPose()
int WorkspaceRepresentation::fillHoles(unsigned int minNeighbors)
{
// copy data
- WorkspaceDataPtr newData(new WorkspaceData(data));
+ WorkspaceDataPtr newData(data->clone());
unsigned int x[6];
int res = 0;
@@ -1528,7 +1528,7 @@ VirtualRobot::WorkspaceRepresentationPtr WorkspaceRepresentation::clone()
memcpy(res->spaceSize,this->spaceSize,sizeof(float)*6);
res->adjustOnOverflow = this->adjustOnOverflow;
- res->data.reset(new WorkspaceData(this->data));
+ res->data.reset(this->data->clone());
return res;
}
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
index de6aab74e5ceb97280db298e28fb0f832eda7bd1..df6cb033b2cefa7550dd6be4a4931098215041c3 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -25,6 +25,7 @@
#include "../VirtualRobotImportExport.h"
#include "WorkspaceData.h"
+#include "WorkspaceDataArray.h"
#include "../MathTools.h"
#include "../XML/FileIO.h"