diff --git a/GraspPlanning/ApproachMovementSurfaceNormal.h b/GraspPlanning/ApproachMovementSurfaceNormal.h
index 99deeeb43c659e2f88fba2ecd751e54624fe10e0..c364356e0add237300d88a3bf21420447ef5cbaf 100644
--- a/GraspPlanning/ApproachMovementSurfaceNormal.h
+++ b/GraspPlanning/ApproachMovementSurfaceNormal.h
@@ -55,10 +55,10 @@ namespace GraspStudio
*/
ApproachMovementSurfaceNormal(VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef, const std::string& graspPreshape = "", float maxRandDist = 0.0f);
//! destructor
- virtual ~ApproachMovementSurfaceNormal();
+ ~ApproachMovementSurfaceNormal() override;
//! Creates a new pose for approaching
- virtual Eigen::Matrix4f createNewApproachPose();
+ Eigen::Matrix4f createNewApproachPose() override;
//! Returns a position with normal on the surface of the object
bool getPositionOnObject(Eigen::Vector3f& storePos, Eigen::Vector3f& storeApproachDir);
diff --git a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
index 7a38f08d10de2f5a588c7a4ad8d317277c3829ac..097a33abf0126ffbe94e232df50bfa7ecca88aee 100644
--- a/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
+++ b/GraspPlanning/GraspPlanner/GenericGraspPlanner.h
@@ -52,7 +52,7 @@ namespace GraspStudio
GenericGraspPlanner(VirtualRobot::GraspSetPtr graspSet, GraspStudio::GraspQualityMeasurePtr graspQuality, GraspStudio::ApproachMovementGeneratorPtr approach, float minQuality = 0.0f, bool forceClosure = true);
// destructor
- virtual ~GenericGraspPlanner();
+ ~GenericGraspPlanner() override;
/*!
Creates new grasps.
@@ -60,7 +60,7 @@ namespace GraspStudio
\param timeOutMS The time out in milliseconds. Planning is stopped when this time is exceeded. Disabled when zero.
\return Number of generated grasps.
*/
- virtual int plan(int nrGrasps, int timeOutMS = 0, VirtualRobot::SceneObjectSetPtr obstacles = VirtualRobot::SceneObjectSetPtr());
+ int plan(int nrGrasps, int timeOutMS = 0, VirtualRobot::SceneObjectSetPtr obstacles = VirtualRobot::SceneObjectSetPtr()) override;
VirtualRobot::EndEffector::ContactInfoVector getContacts() const;
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
index c28b8af8d0f2336e5d67d7b47d460fc194c19559..e53479bd152f999c1ed709833571a61bec04c8d8 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
@@ -48,7 +48,7 @@ namespace GraspStudio
GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8);
// destructor
- virtual ~GraspQualityMeasure();
+ ~GraspQualityMeasure() override;
/*
@@ -62,9 +62,9 @@ namespace GraspStudio
virtual VirtualRobot::MathTools::ContactPoint getSampledObjectPointsCenter();
- virtual std::string getName();
+ std::string getName() override;
- virtual bool isValid();
+ bool isValid() override;
virtual ContactConeGeneratorPtr getConeGenerator();
protected:
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
index 4406d4ebccdc923ef180bbb396d582b0dd062950..a6a4ee0cc39164d0d7994e443becaa351f6ba48c 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h
@@ -47,7 +47,7 @@ namespace GraspStudio
GraspQualityMeasureWrenchSpace(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8);
// destructor
- ~GraspQualityMeasureWrenchSpace();
+ ~GraspQualityMeasureWrenchSpace() override;
/*!
@@ -63,7 +63,7 @@ namespace GraspStudio
with f_max_ows = max distance of OWS hull center to one of its facets
-> also known as "epsilon" quality == radius of larges enclosing 6D ball
*/
- virtual float getGraspQuality();
+ float getGraspQuality() override;
/*!
Volume grasp quality ratio of GWS volume / OWS volume
@@ -74,7 +74,7 @@ namespace GraspStudio
/*
Checks if wrench space origin is inside GWS-Hull
*/
- virtual bool isGraspForceClosure();
+ bool isGraspForceClosure() override;
/*
Returns the internally calculated convex hull object (ObjectWrenchSpace)
@@ -118,14 +118,14 @@ namespace GraspStudio
the contact points are normalized by subtracting the COM
the contact normals are normalize to unit length
*/
- virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints);
- virtual void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints);
+ void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints) override;
+ void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints) override;
- virtual bool calculateGraspQuality();
- virtual bool calculateObjectProperties();
+ bool calculateGraspQuality() override;
+ bool calculateObjectProperties() override;
//! Returns description of this object
- virtual std::string getName();
+ std::string getName() override;
virtual float getOWSMinOffset()
{
diff --git a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h
index 1cea3ed1646c7ce0bce9caad8121b1cf83fe64d5..873eff589f45df39b4e873028a43dc6e162770c6 100644
--- a/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h
+++ b/GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h
@@ -49,7 +49,7 @@ namespace GraspStudio
CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr convHull, bool useFirst3Coords = true);
CoinConvexHullVisualization(VirtualRobot::MathTools::ConvexHull3DPtr convHull);
- ~CoinConvexHullVisualization();
+ ~CoinConvexHullVisualization() override;
SoSeparator* getCoinVisualization();
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
index e4b1d8b4dc49f66fba325e70b110e875c42f5349..489274dea175d3ca85e227de20e1ef00ff424dcb 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
@@ -40,7 +40,7 @@ class GraspPlannerWindow : public QMainWindow
Q_OBJECT
public:
GraspPlannerWindow(std::string& robotFile, std::string& eefName, std::string& preshape, std::string& objectFile);
- ~GraspPlannerWindow();
+ ~GraspPlannerWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -50,7 +50,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
diff --git a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
index 74eb300b687daec5769498a96c2daec19e382325..7617d6ce7890397d996b0f6afa0786725cf19ee3 100644
--- a/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
+++ b/GraspPlanning/examples/GraspQuality/GraspQualityWindow.h
@@ -37,7 +37,7 @@ class GraspQualityWindow : public QMainWindow
Q_OBJECT
public:
GraspQualityWindow(std::string& robotFile, std::string& objectFile);
- ~GraspQualityWindow();
+ ~GraspQualityWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -47,7 +47,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
diff --git a/MotionPlanning/CSpace/CSpacePath.h b/MotionPlanning/CSpace/CSpacePath.h
index aa60c904f5cc3028a2365fd3b249675341f2b808..a7d206331fddf82abe4c2348a09ff24e34d83970 100644
--- a/MotionPlanning/CSpace/CSpacePath.h
+++ b/MotionPlanning/CSpace/CSpacePath.h
@@ -54,7 +54,7 @@ namespace Saba
CSpacePath(CSpacePtr cspace, const std::string& name = "");
//! Destructor
- virtual ~CSpacePath();
+ ~CSpacePath() override;
@@ -70,7 +70,7 @@ namespace Saba
Return euclidean c-space length of complete path.
Any weights that may be defined in the corresponding c-space are considered.
*/
- virtual float getLength() const;
+ float getLength() const override;
/*!
Return euclidean c-space length of complete path
@@ -93,7 +93,7 @@ namespace Saba
If storeIndex!=NULL the index of the last path point is stored.
This method consideres weighting of c-space dimensions!
*/
- virtual void interpolate(float t, Eigen::VectorXf& storePos, int* storeIndex = NULL) const;
+ void interpolate(float t, Eigen::VectorXf& storePos, int* storeIndex = NULL) const override;
//! return time t (0<=t<=1) for path entry with number nr
virtual float getTime(unsigned int nr);
diff --git a/MotionPlanning/CSpace/CSpaceSampled.h b/MotionPlanning/CSpace/CSpaceSampled.h
index de5c71a2694f737a975bf7eb2bcba7343109d9b4..5edc250fd8a861213c0bc1e5d460bdf82574ff29 100644
--- a/MotionPlanning/CSpace/CSpaceSampled.h
+++ b/MotionPlanning/CSpace/CSpaceSampled.h
@@ -71,7 +71,7 @@ namespace Saba
*/
CSpaceSampled(VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs = 50000, unsigned int randomSeed = 0);
- virtual ~CSpaceSampled();
+ ~CSpaceSampled() override;
//! sets sampling step size (used when adding new paths in CSpace)
void setSamplingSize(float fSize);
@@ -90,7 +90,7 @@ namespace Saba
- virtual CSpacePtr clone(VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed = 0);
+ CSpacePtr clone(VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed = 0) override;
/*!
Checks the middle configuration and when it is not invalid (in collision or constraints are violated) the path is split
@@ -98,13 +98,13 @@ namespace Saba
Recursion is performed maximal recursionMaxDepth times, since an array of temporary variables is used,
in order to avoid slow allocating/deallocating of memory.
*/
- bool isPathValid(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2);
+ bool isPathValid(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2) override;
/*!
Create a path from start to goal without any checks.
Intermediate configurations are added according to the current implementation of the cspace.
*/
- virtual CSpacePathPtr createPath(const Eigen::VectorXf& start, const Eigen::VectorXf& goal);
+ CSpacePathPtr createPath(const Eigen::VectorXf& start, const Eigen::VectorXf& goal) override;
/*!
Create a path from start to the goal configuration.
@@ -113,7 +113,7 @@ namespace Saba
\param goal The goal
\param storeAddedLength The length of the valid path is stored here (1.0 means the complete path from start to goal was valid)
*/
- virtual CSpacePathPtr createPathUntilInvalid(const Eigen::VectorXf& start, const Eigen::VectorXf& goal, float& storeAddedLength);
+ CSpacePathPtr createPathUntilInvalid(const Eigen::VectorXf& start, const Eigen::VectorXf& goal, float& storeAddedLength) override;
protected:
diff --git a/MotionPlanning/Planner/BiRrt.h b/MotionPlanning/Planner/BiRrt.h
index 8c1ab8a1279bd21fd796b6c8a596bac5dbc3ce84..551a03eb1f4874eded211e4a0c28988d924ffbd2 100644
--- a/MotionPlanning/Planner/BiRrt.h
+++ b/MotionPlanning/Planner/BiRrt.h
@@ -49,30 +49,30 @@ namespace Saba
\param modeB Specify the RRT method that should be used to build the second tree
*/
BiRrt(CSpacePtr cspace, RrtMethod modeA = eConnect, RrtMethod modeB = eConnect, float samplingSize = -1);
- virtual ~BiRrt();
+ ~BiRrt() override;
/*!
do the planning (blocking method)
\return true if solution was found, otherwise false
*/
- virtual bool plan(bool bQuiet = false);
+ bool plan(bool bQuiet = false) override;
- virtual void printConfig(bool printOnlyParams = false);
+ void printConfig(bool printOnlyParams = false) override;
- virtual void reset();
+ void reset() override;
//! set start configuration
- virtual bool setStart(const Eigen::VectorXf& c);
+ bool setStart(const Eigen::VectorXf& c) override;
//! set goal configuration
- virtual bool setGoal(const Eigen::VectorXf& c);
+ bool setGoal(const Eigen::VectorXf& c) override;
CSpaceTreePtr getTree2();
protected:
- virtual bool createSolution(bool bQuiet = false);
+ bool createSolution(bool bQuiet = false) override;
CSpaceTreePtr tree2; //!< the second tree
diff --git a/MotionPlanning/Planner/GraspIkRrt.h b/MotionPlanning/Planner/GraspIkRrt.h
index f9f7fcbcca8c8b9faa10bbf1f0b72014078905ab..859f588bf58f642268162477dfae595f3f63b4b1 100644
--- a/MotionPlanning/Planner/GraspIkRrt.h
+++ b/MotionPlanning/Planner/GraspIkRrt.h
@@ -52,22 +52,22 @@ namespace Saba
\param probabSampleGoal Probability with which a goal config is created during planning loop.
*/
GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::AdvancedIKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal = 0.1f);
- virtual ~GraspIkRrt();
+ ~GraspIkRrt() override;
/*!
do the planning (blocking method)
\return true if solution was found, otherwise false
*/
- virtual bool plan(bool bQuiet = false);
+ bool plan(bool bQuiet = false) override;
- virtual void printConfig(bool printOnlyParams = false);
+ void printConfig(bool printOnlyParams = false) override;
//! This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown
- virtual bool setGoal(const Eigen::VectorXf& c);
+ bool setGoal(const Eigen::VectorXf& c) override;
//! reset the planner
- virtual void reset();
+ void reset() override;
protected:
diff --git a/MotionPlanning/Planner/GraspRrt.h b/MotionPlanning/Planner/GraspRrt.h
index b09c364158f25b090cbd38294ee98c37857b1db5..87fe61bb96b49873fefda28eb346b1b291579d0e 100644
--- a/MotionPlanning/Planner/GraspRrt.h
+++ b/MotionPlanning/Planner/GraspRrt.h
@@ -77,23 +77,23 @@ namespace Saba
float probabGraspHypothesis = 0.1f,
float graspQualityMinScore = 0.01f);
- virtual ~GraspRrt();
+ ~GraspRrt() override;
/*!
do the planning (blocking method)
\return true if solution was found, otherwise false
*/
- virtual bool plan(bool bQuiet = false);
+ bool plan(bool bQuiet = false) override;
- virtual void printConfig(bool printOnlyParams = false);
- virtual bool setStart(const Eigen::VectorXf& c);
+ void printConfig(bool printOnlyParams = false) override;
+ bool setStart(const Eigen::VectorXf& c) override;
//! This is not allowed here, since we sample goal configurations during planning: If called an exception is thrown
- virtual bool setGoal(const Eigen::VectorXf& c);
+ bool setGoal(const Eigen::VectorXf& c) override;
//! reset the planner
- virtual void reset();
+ void reset() override;
/*!
Returns the internal representation of the pose sphere.
@@ -237,7 +237,7 @@ namespace Saba
VirtualRobot::SceneObjectSetPtr graspCollisionObjects; //!< These objects are considered as obstacles when closing the hand. The targetObject is handled explicitly and must not be part of these object set.
- virtual Rrt::ExtensionResult connectComplete(Eigen::VectorXf& c, CSpaceTreePtr tree, int& storeLastAddedID);
+ Rrt::ExtensionResult connectComplete(Eigen::VectorXf& c, CSpaceTreePtr tree, int& storeLastAddedID) override;
void printGraspInfo(GraspInfo& GrInfo);
diff --git a/MotionPlanning/Planner/Rrt.h b/MotionPlanning/Planner/Rrt.h
index f78ca9b714598f179fb805f400c36fcb9954159a..bd2f1599d968e846783aa21d62516f5407f6214b 100644
--- a/MotionPlanning/Planner/Rrt.h
+++ b/MotionPlanning/Planner/Rrt.h
@@ -71,7 +71,7 @@ namespace Saba
\param probabilityExtendToGoal Specify how often the goal node should be used instead of a random config (value must be between 0 and 1)
*/
Rrt(CSpacePtr cspace, RrtMethod mode = eConnect, float probabilityExtendToGoal = 0.1f, float samplingSize = -1);
- virtual ~Rrt();
+ ~Rrt() override;
/*!
Do the planning (blocking method). On success the Rrt can be accessed with the getTree() method and the
@@ -79,17 +79,17 @@ namespace Saba
\param bQuiet Print some info or not.
\return true if solution was found, otherwise false
*/
- virtual bool plan(bool bQuiet = false);
+ bool plan(bool bQuiet = false) override;
- virtual void printConfig(bool printOnlyParams = false);
+ void printConfig(bool printOnlyParams = false) override;
- virtual void reset();
+ void reset() override;
//! set start configuration
- virtual bool setStart(const Eigen::VectorXf& c);
+ bool setStart(const Eigen::VectorXf& c) override;
//! set goal configuration
- virtual bool setGoal(const Eigen::VectorXf& c);
+ bool setGoal(const Eigen::VectorXf& c) override;
void setProbabilityExtendToGoal(float p);
@@ -97,7 +97,7 @@ namespace Saba
protected:
- virtual bool createSolution(bool bQuiet = false);
+ bool createSolution(bool bQuiet = false) override;
CSpaceTreePtr tree; //!< the rrt on which are operating
diff --git a/MotionPlanning/PostProcessing/ElasticBandProcessor.h b/MotionPlanning/PostProcessing/ElasticBandProcessor.h
index f881181071c87affd500524e48eaafa89fb11409..1422168a3a669fdeb13ddc79a3da386b2b1e1c10 100644
--- a/MotionPlanning/PostProcessing/ElasticBandProcessor.h
+++ b/MotionPlanning/PostProcessing/ElasticBandProcessor.h
@@ -45,10 +45,10 @@ namespace Saba
VirtualRobot::RobotNodePtr node, // the distance for this node is considered
VirtualRobot::SceneObjectSetPtr obstacles, // these obstacles are considered for path smoothing
bool verbose = false);
- virtual ~ElasticBandProcessor();
+ ~ElasticBandProcessor() override;
//! A wrapper to the standard interface.
- virtual CSpacePathPtr optimize(int optimizeSteps);
+ CSpacePathPtr optimize(int optimizeSteps) override;
//! could also be used to disable specific dimensions
void setWeights (Eigen::VectorXf w);
diff --git a/MotionPlanning/PostProcessing/ShortcutProcessor.h b/MotionPlanning/PostProcessing/ShortcutProcessor.h
index 83bee873a3fd3e9970001ada047f6e0f9ce1b2ba..787b3256cad0b2e9033d3c3fc2762cc75adef211 100644
--- a/MotionPlanning/PostProcessing/ShortcutProcessor.h
+++ b/MotionPlanning/PostProcessing/ShortcutProcessor.h
@@ -38,10 +38,10 @@ namespace Saba
public:
ShortcutProcessor(CSpacePathPtr path, CSpaceSampledPtr cspace, bool verbose = false);
- virtual ~ShortcutProcessor();
+ ~ShortcutProcessor() override;
//! A wrapper to the standard interface. Calls shortenSolutionRandom().
- virtual CSpacePathPtr optimize(int optimizeSteps);
+ CSpacePathPtr optimize(int optimizeSteps) override;
/*!
Creates a shortened CSpace path.
diff --git a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
index f89a1fe21b8d6c057bd8dd93cc1f4d512714dd09..dc1a8537e29fad287dbae42df11e07bf2a984ae0 100644
--- a/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
+++ b/MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h
@@ -45,25 +45,25 @@ namespace Saba
CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, CSpacePtr cspace, const std::string& TCPName);
CoinRrtWorkspaceVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr robotNodeSet, const std::string& TCPName);
- ~CoinRrtWorkspaceVisualization();
+ ~CoinRrtWorkspaceVisualization() override;
/*!
Add visualization of a path in cspace.
*/
- virtual bool addCSpacePath(CSpacePathPtr path, RrtWorkspaceVisualization::ColorSet colorSet = eBlue);
+ bool addCSpacePath(CSpacePathPtr path, RrtWorkspaceVisualization::ColorSet colorSet = eBlue) override;
//void setPathStyle(float lineSize = 4.0f, float nodeSize= 15.0f, float renderComplexity = 1.0f);
/*!
Add visualization of a tree (e.g an RRT) in cspace.
*/
- virtual bool addTree(CSpaceTreePtr tree, RrtWorkspaceVisualization::ColorSet colorSet = eRed);
+ bool addTree(CSpaceTreePtr tree, RrtWorkspaceVisualization::ColorSet colorSet = eRed) override;
//void setTreeStyle(float lineSize = 1.0f, float nodeSize= 15.0f, float renderComplexity = 0.1f);
/*!
Add visualization of a configuration in cspace.
*/
- virtual bool addConfiguration(const Eigen::VectorXf& c, RrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f);
+ bool addConfiguration(const Eigen::VectorXf& c, RrtWorkspaceVisualization::ColorSet colorSet = eGreen, float nodeSizeFactor = 1.0f) override;
/*!
Set the custom line and node color. Does not affect already added trees or paths.
@@ -73,7 +73,7 @@ namespace Saba
/*!
Clears all visualizations.
*/
- virtual void reset();
+ void reset() override;
SoSeparator* getCoinVisualization();
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
index 43cead050e23024327d3f2a13b9a0452534eea22..166136c2539d7599ef0322350dd9d5dcca882473 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.h
@@ -41,7 +41,7 @@ public:
const std::string& rns, const std::string& rnsB, const std::string& eefName, const std::string& eefNameB,
const std::string& colModelRob1, const std::string& colModelRob1B, const std::string& colModelRob2, const std::string& colModelRob2B,
const std::string& colModelEnv);
- ~GraspRrtWindow();
+ ~GraspRrtWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -52,7 +52,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void loadSceneWindow();
diff --git a/MotionPlanning/examples/IKRRT/IKRRTWindow.h b/MotionPlanning/examples/IKRRT/IKRRTWindow.h
index 4f015e1f4266f66b8800d5a9d94d088b4e54c917..cab6304afd518048e4febcf06ac0a556c97ad605 100644
--- a/MotionPlanning/examples/IKRRT/IKRRTWindow.h
+++ b/MotionPlanning/examples/IKRRT/IKRRTWindow.h
@@ -36,7 +36,7 @@ class IKRRTWindow : public QMainWindow
Q_OBJECT
public:
IKRRTWindow(std::string& sceneFile, std::string& reachFile, std::string& rns, std::string& eef, std::string& colModel, std::string& colModelRob);
- ~IKRRTWindow();
+ ~IKRRTWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -47,7 +47,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
diff --git a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.h b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.h
index 4a9b5eb671381ae4326ad01d1d4419322e0bf7d0..f7babb0fbfe92669cf7e95b4dabb491a609decad 100644
--- a/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.h
+++ b/MotionPlanning/examples/MultiThreadedPlanning/MTPlanningWindow.h
@@ -34,7 +34,7 @@ class MTPlanningWindow : public QMainWindow
public:
MTPlanningWindow();
- ~MTPlanningWindow();
+ ~MTPlanningWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -44,7 +44,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
//void loadRobot();
void buildScene();
diff --git a/MotionPlanning/examples/PlatformDemo/PlatformWindow.h b/MotionPlanning/examples/PlatformDemo/PlatformWindow.h
index 6022f4720a8fa8212936b2dc6892c0aa533d81a2..9e408e6f901358138dbd5f7239776ff9aa89eaa6 100644
--- a/MotionPlanning/examples/PlatformDemo/PlatformWindow.h
+++ b/MotionPlanning/examples/PlatformDemo/PlatformWindow.h
@@ -41,7 +41,7 @@ public:
const std::string& rns,
const std::string& colModelRob,
const std::string& colModelEnv);
- ~PlatformWindow();
+ ~PlatformWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -52,7 +52,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void loadSceneWindow();
diff --git a/MotionPlanning/examples/RrtGui/RrtGuiWindow.h b/MotionPlanning/examples/RrtGui/RrtGuiWindow.h
index eae7ad59e8c41cdd8f07edb086b25b91233e3059..c237192a2f933d382dcba370b8f0c74cbd52834c 100644
--- a/MotionPlanning/examples/RrtGui/RrtGuiWindow.h
+++ b/MotionPlanning/examples/RrtGui/RrtGuiWindow.h
@@ -37,7 +37,7 @@ class RrtGuiWindow : public QMainWindow
public:
RrtGuiWindow(const std::string& sceneFile, const std::string& sConf, const std::string& gConf, const std::string& rns,
const std::string& colModelRob1, const std::string& colModelRob2, const std::string& colModelEnv);
- ~RrtGuiWindow();
+ ~RrtGuiWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -48,7 +48,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void loadSceneWindow();
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index 7024b6fe1a64bc3886dd1f373355994137e8d9e2..daa4a3f6065745e62c5d12480916cb7773d20c5e 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -47,7 +47,7 @@ namespace SimDynamics
public:
BulletEngineConfig();
- virtual ~BulletEngineConfig() {}
+ ~BulletEngineConfig() override {}
// global setup values
btScalar bulletObjectRestitution;
@@ -86,19 +86,19 @@ namespace SimDynamics
/*!
*/
- virtual ~BulletEngine();
+ ~BulletEngine() override;
- virtual bool addObject(DynamicsObjectPtr o);
- virtual bool removeObject(DynamicsObjectPtr o);
+ bool addObject(DynamicsObjectPtr o) override;
+ bool removeObject(DynamicsObjectPtr o) override;
- virtual bool addRobot(DynamicsRobotPtr r);
- virtual bool removeRobot(DynamicsRobotPtr r);
+ bool addRobot(DynamicsRobotPtr r) override;
+ bool removeRobot(DynamicsRobotPtr r) override;
/*!
Initialize the engine with this configuration.
\param config Either a standard init (could be NULL), or if config is of type BulletEngineConfig, Bullet specific parameters will be considered.
*/
- virtual bool init(DynamicsEngineConfigPtr config);
+ bool init(DynamicsEngineConfigPtr config) override;
virtual bool init(BulletEngineConfigPtr config);
virtual bool cleanup();
@@ -109,7 +109,7 @@ namespace SimDynamics
Set floor
\param friction If <=0.0, the standard friction parameter for novel objects is used.
*/
- virtual void createFloorPlane(const Eigen::Vector3f& pos, const Eigen::Vector3f& up, float friction = 0.0f);
+ void createFloorPlane(const Eigen::Vector3f& pos, const Eigen::Vector3f& up, float friction = 0.0f) override;
/*!
dt and fixedTimeStep are given in seconds.
@@ -118,7 +118,7 @@ namespace SimDynamics
btDynamicsWorld* getBulletWorld();
- virtual std::vector<DynamicsEngine::DynamicsContactInfo> getContacts();
+ std::vector<DynamicsEngine::DynamicsContactInfo> getContacts() override;
void print();
@@ -133,8 +133,8 @@ namespace SimDynamics
*/
double getSimTime();
- virtual bool attachObjectToRobot(DynamicsRobotPtr r, const std::string& nodeName, DynamicsObjectPtr object);
- virtual bool detachObjectFromRobot(DynamicsRobotPtr r, DynamicsObjectPtr object);
+ bool attachObjectToRobot(DynamicsRobotPtr r, const std::string& nodeName, DynamicsObjectPtr object) override;
+ bool detachObjectFromRobot(DynamicsRobotPtr r, DynamicsObjectPtr object) override;
/*!
@@ -158,7 +158,7 @@ namespace SimDynamics
{
engine = e;
}
- virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
+ bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override
{
VR_ASSERT(engine);
VR_ASSERT(static_cast<btCollisionObject*>(proxy0->m_clientObject));
@@ -198,8 +198,8 @@ namespace SimDynamics
// btActionInterface interface
public:
- void updateAction(btCollisionWorld *collisionWorld, btScalar deltaTimeStep);
- void debugDraw(btIDebugDraw *debugDrawer);
+ void updateAction(btCollisionWorld *collisionWorld, btScalar deltaTimeStep) override;
+ void debugDraw(btIDebugDraw *debugDrawer) override;
};
typedef boost::shared_ptr<BulletEngine> BulletEnginePtr;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
index 1f5c73c5a6001b6ddc9d03f710aea585e561b143..2cc810c0d6797055bfd468f7de6328f93010d702 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
@@ -40,12 +40,12 @@ namespace SimDynamics
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
BulletEngineFactory();
- virtual ~BulletEngineFactory();
+ ~BulletEngineFactory() override;
- virtual DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr());
+ DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr()) override;
- virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o);
- virtual DynamicsRobotPtr createRobot(VirtualRobot::RobotPtr robot);
+ DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o) override;
+ DynamicsRobotPtr createRobot(VirtualRobot::RobotPtr robot) override;
// AbstractFactoryMethod
public:
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index 73923485090aeb0d707cc6e8ba075aa797697ec6..3a0e814c04121cdeb121a7854d2a2c1ac59e4a6a 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -42,7 +42,7 @@ namespace SimDynamics
/*!
*/
- virtual ~BulletObject();
+ ~BulletObject() override;
boost::shared_ptr<btRigidBody> getRigidBody();
@@ -51,23 +51,23 @@ namespace SimDynamics
/*!
Set world position [MM].
*/
- virtual void setPosition(const Eigen::Vector3f& posMM);
+ void setPosition(const Eigen::Vector3f& posMM) override;
/*!
Set world pose [mm].
*/
- virtual void setPose(const Eigen::Matrix4f& pose);
+ void setPose(const Eigen::Matrix4f& pose) override;
Eigen::Vector3f getCom()
{
return com;
}
- virtual Eigen::Vector3f getLinearVelocity();
- virtual Eigen::Vector3f getAngularVelocity();
+ Eigen::Vector3f getLinearVelocity() override;
+ Eigen::Vector3f getAngularVelocity() override;
- virtual void setLinearVelocity(const Eigen::Vector3f& vel);
- virtual void setAngularVelocity(const Eigen::Vector3f& vel);
+ void setLinearVelocity(const Eigen::Vector3f& vel) override;
+ void setAngularVelocity(const Eigen::Vector3f& vel) override;
//! This is the world pose which is set by bullet
Eigen::Matrix4f getComGlobal();
@@ -76,20 +76,20 @@ namespace SimDynamics
* \brief applyForce Applies an external force on this object. The force is applied at the CoM position.
* \param force The force to apply (value with respect to one second). The force will be deleted after one simulation step.
*/
- virtual void applyForce(const Eigen::Vector3f& force);
+ void applyForce(const Eigen::Vector3f& force) override;
/*!
* \brief applyTorque Applies an external torque on this object. The torque is applied at the CoM position.
* \param torque The torque to apply (value with respect to one second). The torque will be deleted after one simulation step.
*/
- virtual void applyTorque(const Eigen::Vector3f& torque);
+ void applyTorque(const Eigen::Vector3f& torque) override;
- virtual void setSimType(VirtualRobot::SceneObject::Physics::SimulationType s);
+ void setSimType(VirtualRobot::SceneObject::Physics::SimulationType s) override;
/*!
* \brief activate If object is sleeping, we can activate it here.
*/
- virtual void activate();
+ void activate() override;
//! All object's sizes are scaled by this factor for bullet. (Small objects (<5cm) do not work well with bullet).
static float ScaleFactor;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.h
index 30f8718e15686d51c53e355713d14ddeecc593ee..7e832732ffcf2aa71e49af6f9a694ebee7cdfe55 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GLDebugDrawer.h
@@ -12,25 +12,25 @@ class GLDebugDrawer : public btIDebugDraw
public:
GLDebugDrawer();
- virtual ~GLDebugDrawer();
+ ~GLDebugDrawer() override;
- virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor);
+ void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor) override;
- virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color);
+ void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) override;
- virtual void drawSphere(const btVector3& p, btScalar radius, const btVector3& color);
+ void drawSphere(const btVector3& p, btScalar radius, const btVector3& color) override;
- virtual void drawTriangle(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& color, btScalar alpha);
+ void drawTriangle(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& color, btScalar alpha) override;
- virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color);
+ void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) override;
- virtual void reportErrorWarning(const char* warningString);
+ void reportErrorWarning(const char* warningString) override;
- virtual void draw3dText(const btVector3& location, const char* textString);
+ void draw3dText(const btVector3& location, const char* textString) override;
- virtual void setDebugMode(int debugMode);
+ void setDebugMode(int debugMode) override;
- virtual int getDebugMode() const
+ int getDebugMode() const override
{
return m_debugMode;
}
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.h
index 142379af0a4d977427b0b012b178ebde40802f2f..f440bcae317fb3e195498dcc113e10d2dd17bdf0 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_DialogWindow.h
@@ -87,9 +87,9 @@ public:
m_type = GL_TEXT_CONTROL;
}
- virtual ~GL_TextControl() {}
+ ~GL_TextControl() override {}
- virtual void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime);
+ void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime) override;
};
@@ -114,7 +114,7 @@ public:
m_type = GL_TOGGLE_CONTROL;
}
- virtual void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime);
+ void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime) override;
};
struct GL_SliderControl : public GL_DialogControl
@@ -140,7 +140,7 @@ public:
m_type = GL_SLIDER_CONTROL;
}
- virtual void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime);
+ void draw(int& parentHorPos, int& parentVertPos, btScalar deltaTime) override;
btScalar btGetFraction()
{
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
index ce48906d2f0983c184bd9df47e0ed7bffcecbd4c..8927289ccecffbaf0af794512f69321e20f4672d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_ShapeDrawer.cpp
@@ -213,7 +213,7 @@ public:
{
}
- virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+ void processTriangle(btVector3* triangle, int partId, int triangleIndex) override
{
(void)triangleIndex;
@@ -255,7 +255,7 @@ public:
class TriangleGlDrawcallback : public btInternalTriangleIndexCallback
{
public:
- virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+ void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) override
{
(void)triangleIndex;
(void)partId;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.h
index 54161b3bd486b20b29be398883405b9abc2bc080..360ec95a7e44793f566d422f1c66736f1f3fa550 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GL_Simplex1to4.h
@@ -28,7 +28,7 @@ class GL_Simplex1to4 : public btBU_Simplex1to4
public:
GL_Simplex1to4();
- virtual ~GL_Simplex1to4();
+ ~GL_Simplex1to4() override;
void calcClosest(btScalar* m);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutDemoApplication.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutDemoApplication.h
index d30c7d868010c61047b6453cf7b991dffcd67269..4d21a6752996d0e8048dc351de42cc3c24c57c49 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutDemoApplication.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGL/GlutDemoApplication.h
@@ -26,11 +26,11 @@ public:
BT_DECLARE_ALIGNED_ALLOCATOR();
- void specialKeyboard(int key, int x, int y);
+ void specialKeyboard(int key, int x, int y) override;
- virtual void swapBuffers();
+ void swapBuffers() override;
- virtual void updateModifierKeys();
+ void updateModifierKeys() override;
};
#endif //GLUT_DEMO_APPLICATION_H
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
index a07be7fee1a66ae7e0128b8f1962ae9d040db0d9..a9c10feb20cff943c37f732b1fb726980612c822 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletOpenGLViewer.h
@@ -53,13 +53,13 @@ namespace SimDynamics
{
public:
BulletOpenGLViewer(DynamicsWorldPtr world);
- virtual ~BulletOpenGLViewer();
+ ~BulletOpenGLViewer() override;
- virtual void clientMoveAndDisplay();
- virtual void displayCallback();
- virtual void keyboardCallback(unsigned char key, int x, int y);
- virtual void initPhysics();
- virtual void myinit();
+ void clientMoveAndDisplay() override;
+ void displayCallback() override;
+ void keyboardCallback(unsigned char key, int x, int y) override;
+ void initPhysics() override;
+ void myinit() override;
virtual void enableContraintsDebugDrawing();
protected:
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
index d8d1395f2a504f8dcf222375e62f9f5085d62a9b..4edfe47a8bff7cf2ad85e2f97f679de43d037c33 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.h
@@ -44,7 +44,7 @@ namespace SimDynamics
/*!
*/
- virtual ~BulletRobot();
+ ~BulletRobot() override;
struct LinkInfo
{
@@ -100,20 +100,20 @@ namespace SimDynamics
std::vector<LinkInfo> getLinks();
- virtual void actuateNode(VirtualRobot::RobotNodePtr node, double jointValue);
- virtual void actuateNodeVel(VirtualRobot::RobotNodePtr node, double jointVelocity);
+ void actuateNode(VirtualRobot::RobotNodePtr node, double jointValue) override;
+ void actuateNodeVel(VirtualRobot::RobotNodePtr node, double jointVelocity) override;
/*!
Usually this method is called by the framework in every tick to perform joint actuation.
\param dt Timestep
*/
- virtual void actuateJoints(double dt);
- virtual void updateSensors(double dt);
+ void actuateJoints(double dt) override;
+ void updateSensors(double dt) override;
- virtual double getJointAngle(VirtualRobot::RobotNodePtr rn);
- virtual double getJointSpeed(VirtualRobot::RobotNodePtr rn);
- virtual double getJointTargetSpeed(VirtualRobot::RobotNodePtr rn);
- virtual double getNodeTarget(VirtualRobot::RobotNodePtr node);
+ double getJointAngle(VirtualRobot::RobotNodePtr rn) override;
+ double getJointSpeed(VirtualRobot::RobotNodePtr rn) override;
+ double getJointTargetSpeed(VirtualRobot::RobotNodePtr rn) override;
+ double getNodeTarget(VirtualRobot::RobotNodePtr node) override;
/*!
* \brief getJointTorques retrieves the torques in the given joint.
@@ -147,27 +147,27 @@ namespace SimDynamics
/*!
Returns the CoM pose, which is reported by bullet
*/
- virtual Eigen::Matrix4f getComGlobal(const VirtualRobot::RobotNodePtr& rn);
- virtual Eigen::Vector3f getComGlobal(const VirtualRobot::RobotNodeSetPtr& set);
- virtual Eigen::Vector3f getComVelocityGlobal(const VirtualRobot::RobotNodeSetPtr& set);
+ Eigen::Matrix4f getComGlobal(const VirtualRobot::RobotNodePtr& rn) override;
+ Eigen::Vector3f getComGlobal(const VirtualRobot::RobotNodeSetPtr& set) override;
+ Eigen::Vector3f getComVelocityGlobal(const VirtualRobot::RobotNodeSetPtr& set) override;
/*!
* Returns the linear momentum in Ns for the bodies in the nodeset.
*/
- virtual Eigen::Vector3f getLinearMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set);
+ Eigen::Vector3f getLinearMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set) override;
/*!
* Returns the angular momentum in Nms for the bodies in the nodeset
*/
- virtual Eigen::Vector3f getAngularMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set);
+ Eigen::Vector3f getAngularMomentumGlobal(const VirtualRobot::RobotNodeSetPtr& set) override;
/*!
* Returns the angular momentum in Nms for the bodies in the nodeset relative to the CoM
*/
- virtual Eigen::Vector3f getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set);
+ Eigen::Vector3f getAngularMomentumLocal(const VirtualRobot::RobotNodeSetPtr& set) override;
// experimental...
- virtual void ensureKinematicConstraints();
+ void ensureKinematicConstraints() override;
/*!
Returns link where the given node is the joint node.
@@ -203,10 +203,10 @@ namespace SimDynamics
void buildBulletModels(bool enableJointMotors);
//! creates a link and attaches object to internal data structure
- virtual bool attachObject(const std::string& nodeName, DynamicsObjectPtr object);
+ bool attachObject(const std::string& nodeName, DynamicsObjectPtr object) override;
LinkInfoPtr attachObjectLink(const std::string& nodeName, DynamicsObjectPtr object);
- virtual bool detachObject(DynamicsObjectPtr object);
+ bool detachObject(DynamicsObjectPtr object) override;
//void createLink( VirtualRobot::RobotNodePtr bodyA, VirtualRobot::RobotNodePtr joint, VirtualRobot::RobotNodePtr bodyB, Eigen::Matrix4f &trafoA2J, Eigen::Matrix4f &trafoJ2B, bool enableJointMotors = true );
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
index e7c8742fc18982bcc7be568cce9622bb5dfbb699..1d179e5477cda3158de1b423dca06a392ecb6f76 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxCollisionDispatcher.h
@@ -42,9 +42,9 @@ namespace SimDynamics
struct SIMDYNAMICS_IMPORT_EXPORT SimoxCollisionDispatcher : public btCollisionDispatcher
{
SimoxCollisionDispatcher(BulletEngine* engine, btCollisionConfiguration* collisionConfiguration);
- virtual ~SimoxCollisionDispatcher();
- virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1);
- virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1);
+ ~SimoxCollisionDispatcher() override;
+ bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1) override;
+ bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) override;
protected:
BulletEngine* engine;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.h b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.h
index c2e4257ce0193f5acdb72a318555a30489966e36..b573d105fb5a6703b43fbda91eb327eb61fc53f0 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/SimoxMotionState.h
@@ -67,7 +67,7 @@ namespace SimDynamics
/*!
Destructor.
*/
- virtual ~SimoxMotionState();
+ ~SimoxMotionState() override;
/*!
Interface for changing poses of rigid body and VirtualRobot objects.
@@ -75,7 +75,7 @@ namespace SimDynamics
Bullet sets and gets the rigid body world pose using
setWorldTransform() and getWorldTransform().
*/
- virtual void setWorldTransform(const btTransform& worldPose);
+ void setWorldTransform(const btTransform& worldPose) override;
/*!
Interface for getting poses of rigid body and VirtualRobot objects.
@@ -83,7 +83,7 @@ namespace SimDynamics
Bullet sets and gets the rigid body world pose using
setWorldTransform() and getWorldTransform().
*/
- virtual void getWorldTransform(btTransform& worldPose) const;
+ void getWorldTransform(btTransform& worldPose) const override;
/*!
Set center of mass.
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index 92db29307292bb7c13eef550ff8b040822709be6..2207bc3722808d4c43e3a199d43b1be2a58e5e55 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -37,7 +37,7 @@ class SimDynamicsWindow : public QMainWindow
Q_OBJECT
public:
SimDynamicsWindow(std::string& sRobotFilename);
- ~SimDynamicsWindow();
+ ~SimDynamicsWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -47,7 +47,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void buildVisualization();
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 51f2d73a6e3b1906ad45373a57021748f6732320..04d522012455f6db077d9500489424b45d573b4f 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -145,6 +145,33 @@ Import/RobotImporterFactory.cpp
Import/MeshImport/STLReader.cpp
VirtualRobot.cpp
Tools/Gravity.cpp
+
+math/Contact.cpp
+math/ContactList.cpp
+math/Line.cpp
+math/LineStrip.cpp
+math/Bezier.cpp
+math/LinearContinuedBezier.cpp
+math/LineR2.cpp
+math/AbstractFunctionR1R2.cpp
+math/AbstractFunctionR1R3.cpp
+math/AbstractFunctionR2R3.cpp
+math/Plane.cpp
+math/Triangle.cpp
+math/ImplicitPlane.cpp
+math/GaussianObjectModel.cpp
+math/GaussianObjectModelNormals.cpp
+math/ImplicitObjectModel.cpp
+math/GaussianImplicitSurface3D.cpp
+math/GaussianImplicitSurface3DNormals.cpp
+math/DataR3R1.cpp
+math/Helpers.cpp
+math/Index3.cpp
+math/MarchingCubes.cpp
+math/Primitive.cpp
+math/WeightedAverage.cpp
+math/Grid3D.cpp
+math/GridCacheFloat3.cpp
)
SET(INCLUDES
@@ -262,6 +289,40 @@ Import/SimoxXMLFactory.h
Import/RobotImporterFactory.h
Import/MeshImport/STLReader.h
Tools/Gravity.h
+
+math/MathForwardDefinitions.h
+math/Contact.h
+math/ContactList.h
+math/Line.h
+math/LineStrip.h
+math/Bezier.h
+math/LinearContinuedBezier.h
+math/LineR2.h
+math/AbstractFunctionR1R2.h
+math/AbstractFunctionR1R3.h
+math/AbstractFunctionR2R3.h
+math/AbstractFunctionR3R1.h
+math/SimpleAbstractFunctionR1R3.h
+math/SimpleAbstractFunctionR2R3.h
+math/SimpleAbstractFunctionR3R1.h
+math/Plane.h
+math/Triangle.h
+math/ImplicitPlane.h
+math/ImplicitObjectModel.cpp
+math/GaussianObjectModel.h
+math/GaussianObjectModelNormals.h
+math/GaussianImplicitSurface3D.h
+math/GaussianImplicitSurface3DNormals.h
+math/DataR3R1.h
+math/Helpers.h
+math/Index3.h
+math/Array3D.h
+math/MarchingCubes.h
+math/Primitive.h
+math/Grid3D.h
+math/GridCacheFloat3.h
+math/WeightedAverage.h
+
)
if (Simox_USE_RBDL AND RBDL_FOUND)
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
index fc4ede660af84fab43fd77e0c1eb89b312cd5e36..61fb6e12f0e23e177c01114c523e07fd2ea54bac 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.h
@@ -48,10 +48,10 @@ namespace VirtualRobot
friend class CollisionChecker;
CollisionCheckerPQP();
- virtual ~CollisionCheckerPQP();
+ ~CollisionCheckerPQP() override;
- virtual float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL);
- virtual bool checkCollision(CollisionModelPtr model1, CollisionModelPtr model2); //, Eigen::Vector3f *storeContact = NULL);
+ float calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1 = NULL, int* trID2 = NULL) override;
+ bool checkCollision(CollisionModelPtr model1, CollisionModelPtr model2) override; //, Eigen::Vector3f *storeContact = NULL);
/*!
If continuous collision detection (CCD) is supported, this method can be used to detect collisions on the path
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
index 8984ed1fd517d989f9c82833a26ae2db764ffdbc..1ca1559d4f88091efbc5f76d2ca510c11488f71b 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionModelPQP.h
@@ -55,20 +55,20 @@ namespace VirtualRobot
CollisionModelPQP(TriMeshModelPtr modelData, CollisionCheckerPtr colChecker, int id);
/*!Standard Destructor
*/
- virtual ~CollisionModelPQP();
+ ~CollisionModelPQP() override;
boost::shared_ptr<PQP::PQP_Model> getPQPModel()
{
return pqpModel;
}
- virtual void print();
- virtual boost::shared_ptr<CollisionModelImplementation> clone(bool deepCopy = false) const;
+ void print() override;
+ boost::shared_ptr<CollisionModelImplementation> clone(bool deepCopy = false) const override;
protected:
CollisionModelPQP(const CollisionModelPQP& orig);
//! delete all data
- virtual void destroyData();
+ void destroyData() override;
void createPQPModel();
boost::shared_ptr<PQP::PQP_Model> pqpModel;
diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
index c657bd1032f61975459c311e766293b9b54ee81c..69debd2cbb8f7d059d586e6b4a272b38723fe8f2 100644
--- a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
+++ b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
@@ -43,7 +43,7 @@ namespace VirtualRobot
Q_OBJECT
public:
GraspEditorWindow(std::string& objFile, std::string& robotFile, bool embeddedGraspEditor = false);
- virtual ~GraspEditorWindow();
+ ~GraspEditorWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -53,7 +53,7 @@ namespace VirtualRobot
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void loadObject();
diff --git a/VirtualRobot/IK/CoMIK.h b/VirtualRobot/IK/CoMIK.h
index 753a19d820fc666487255b8d1bfd9803f3b2be9e..fc5947986caa1d5c72a0fec278b106a33fe4ce30 100644
--- a/VirtualRobot/IK/CoMIK.h
+++ b/VirtualRobot/IK/CoMIK.h
@@ -46,10 +46,10 @@ namespace VirtualRobot
void setGoal(const Eigen::VectorXf& goal, float tolerance = 5.0f);
Eigen::MatrixXf getJacobianOfCoM(RobotNodePtr node);
- virtual Eigen::MatrixXf getJacobianMatrix();
- virtual Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp); // ignored for CoM IK but needed for interface
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override; // ignored for CoM IK but needed for interface
- virtual Eigen::VectorXf getError(float stepSize = 1.0f);
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
Eigen::VectorXf computeStep(float stepSize);
bool computeSteps(float stepSize, float minumChange, int maxNStep);
@@ -62,11 +62,11 @@ namespace VirtualRobot
bool isValid(const Eigen::VectorXf& v) const;
- virtual bool checkTolerances();
+ bool checkTolerances() override;
void checkImprovements(bool enable);
bool solveIK(float stepSize = 0.2f, float minChange = 0.0f, int maxSteps = 50);
- virtual void print();
+ void print() override;
private:
RobotNodePtr coordSystem;
RobotNodeSetPtr rnsBodies;
diff --git a/VirtualRobot/IK/ConstrainedHierarchicalIK.h b/VirtualRobot/IK/ConstrainedHierarchicalIK.h
index 1b800535b1f1a997ac01c897ffa64955efc40af4..9af85d2b667631a6ae37fedc3dee36e5743d2817 100644
--- a/VirtualRobot/IK/ConstrainedHierarchicalIK.h
+++ b/VirtualRobot/IK/ConstrainedHierarchicalIK.h
@@ -36,8 +36,8 @@ namespace VirtualRobot
public:
ConstrainedHierarchicalIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize = 0.2f, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.1);
- bool initialize();
- bool solveStep();
+ bool initialize() override;
+ bool solveStep() override;
protected:
RobotNodeSetPtr nodeSet;
diff --git a/VirtualRobot/IK/ConstrainedOptimizationIK.h b/VirtualRobot/IK/ConstrainedOptimizationIK.h
index 0e3de4333363f78f8df783f11713568604ca0a9a..fc862a869d357ad19db90c660e52a35d364233a7 100644
--- a/VirtualRobot/IK/ConstrainedOptimizationIK.h
+++ b/VirtualRobot/IK/ConstrainedOptimizationIK.h
@@ -44,9 +44,9 @@ namespace VirtualRobot
public:
ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout = 0.5, float globalTolerance = std::numeric_limits<float>::quiet_NaN());
- bool initialize();
- bool solve(bool stepwise = false);
- bool solveStep();
+ bool initialize() override;
+ bool solve(bool stepwise = false) override;
+ bool solveStep() override;
/**
* This factor limits the interval around the initial robot configuration where random samplings are placed.
diff --git a/VirtualRobot/IK/ConstrainedStackedIK.h b/VirtualRobot/IK/ConstrainedStackedIK.h
index 64e50edf4370164183a4f912c64ed17dd128e355..2d382b90f84f886194f84a9a0801c3be7d488689 100644
--- a/VirtualRobot/IK/ConstrainedStackedIK.h
+++ b/VirtualRobot/IK/ConstrainedStackedIK.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
ConstrainedStackedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize = 0.2f, int maxIterations = 1000,
JacobiProvider::InverseJacobiMethod method = JacobiProvider::eSVD);
- bool initialize();
+ bool initialize() override;
- virtual bool solveStep();
+ bool solveStep() override;
protected:
RobotNodeSetPtr nodeSet;
diff --git a/VirtualRobot/IK/Constraint.h b/VirtualRobot/IK/Constraint.h
index 3a6e540f9554f462bce1414f17a573e921404944..9804612d9df44e739c4b56a44d0bdd6e45cde4f8 100644
--- a/VirtualRobot/IK/Constraint.h
+++ b/VirtualRobot/IK/Constraint.h
@@ -75,10 +75,10 @@ namespace VirtualRobot
virtual Eigen::VectorXf optimizationGradient(unsigned int id);
// Interface for Jacobian-based solvers (default implementations)
- virtual Eigen::MatrixXf getJacobianMatrix();
- virtual Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
- virtual Eigen::VectorXf getError(float stepSize = 1.0f);
- virtual bool checkTolerances();
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override;
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
+ bool checkTolerances() override;
virtual bool usingCollisionModel();
diff --git a/VirtualRobot/IK/DifferentialIK.h b/VirtualRobot/IK/DifferentialIK.h
index e9c3465e53b28c3ba0ecbccbf98c68baa8e21f5d..8c4621a62382a41413fc6e525d8019402513800b 100644
--- a/VirtualRobot/IK/DifferentialIK.h
+++ b/VirtualRobot/IK/DifferentialIK.h
@@ -177,18 +177,18 @@ namespace VirtualRobot
see <a href="http://graphics.ucsd.edu/courses/cse169_w05/CSE169_13.ppt">this lecture</a>.
*/
virtual Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp, IKSolver::CartesianSelection mode);
- virtual Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override;
virtual Eigen::MatrixXf getJacobianMatrix(IKSolver::CartesianSelection mode);
/*!
Computes the complete Jacobian that considers all defined TCPs and goal poses.
*/
- virtual Eigen::MatrixXf getJacobianMatrix();
+ Eigen::MatrixXf getJacobianMatrix() override;
/*!
Computes the complete error vector, considering all TCPs and goals.
*/
- virtual Eigen::VectorXf getError(float stepSize = 1.0f);
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
void updateError(Eigen::VectorXf& error, float stepSize = 1.0f);
/*! @brief Returns the pseudo inverse of the Jacobian matrix for a given tcp of the robot.
@@ -279,7 +279,7 @@ namespace VirtualRobot
//! When considering large errors, the translational part can be cut to this length. Set to <= 0 to ignore cutting (standard)
virtual void setMaxPositionStep(float s);
- virtual bool checkTolerances();
+ bool checkTolerances() override;
/*!
Initializes the internal data structures according to setGoal setup.
@@ -287,7 +287,7 @@ namespace VirtualRobot
*/
virtual void initialize();
- virtual void print();
+ void print() override;
protected:
virtual void setNRows();
diff --git a/VirtualRobot/IK/GenericIKSolver.h b/VirtualRobot/IK/GenericIKSolver.h
index bf99b32253027ecbe5fb96d19af34e6af37a28de..3992669718daf7f0c5ce01ab297ab4638ed58bce 100644
--- a/VirtualRobot/IK/GenericIKSolver.h
+++ b/VirtualRobot/IK/GenericIKSolver.h
@@ -50,7 +50,7 @@ namespace VirtualRobot
\param maxLoops How often should we try.
\return true on success
*/
- virtual bool solve(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All, int maxLoops = 1);
+ bool solve(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All, int maxLoops = 1) override;
/*!
This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
@@ -59,8 +59,8 @@ namespace VirtualRobot
\param maxLoops How often should we try.
\return On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr
*/
- virtual GraspPtr solve(ManipulationObjectPtr object, CartesianSelection selection = All, int maxLoops = 1);
- virtual bool solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection = All, int maxLoops = 1);
+ GraspPtr solve(ManipulationObjectPtr object, CartesianSelection selection = All, int maxLoops = 1) override;
+ bool solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection = All, int maxLoops = 1) override;
void setupJacobian(float stepSize, int maxLoops);
@@ -76,7 +76,7 @@ namespace VirtualRobot
//! This method is called by the constructor and can be used in derived classes for initialization.
virtual void _init();
- virtual bool _sampleSolution(const Eigen::Matrix4f& globalPose, CartesianSelection selection, int maxLoops = 1);
+ bool _sampleSolution(const Eigen::Matrix4f& globalPose, CartesianSelection selection, int maxLoops = 1) override;
RobotNodePtr coordSystem;
JacobiProvider::InverseJacobiMethod invJacMethod;
diff --git a/VirtualRobot/IK/JointLimitAvoidanceJacobi.h b/VirtualRobot/IK/JointLimitAvoidanceJacobi.h
index 96505499d01e29b865a12deeb990798d45318a49..214942ab3737c701895d3ee3fa241d2d2ced9a8d 100644
--- a/VirtualRobot/IK/JointLimitAvoidanceJacobi.h
+++ b/VirtualRobot/IK/JointLimitAvoidanceJacobi.h
@@ -41,20 +41,20 @@ namespace VirtualRobot
JointLimitAvoidanceJacobi(RobotNodeSetPtr rns, JacobiProvider::InverseJacobiMethod invJacMethod = JacobiProvider::eSVD);
- virtual Eigen::MatrixXf getJacobianMatrix();
- virtual Eigen::MatrixXf getJacobianMatrix(VirtualRobot::SceneObjectPtr tcp);
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(VirtualRobot::SceneObjectPtr tcp) override;
/*!
Computes the complete error vector that is given by the distance to the center of the joint limits.
Translational Joints are ignored (error = 0)
\param stepSize The error can be reduced by this factor.
*/
- virtual Eigen::VectorXf getError(float stepSize = 1.0f);
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
/*!
* Not used, only implemented because of superclass JacobiProvider, always returns true
*/
- bool checkTolerances();
+ bool checkTolerances() override;
protected:
std::vector<RobotNodePtr> nodes;
diff --git a/VirtualRobot/IK/PoseQualityExtendedManipulability.h b/VirtualRobot/IK/PoseQualityExtendedManipulability.h
index c2ddd94a591247c7b28254c46009ae8d3f63c0a4..1b62015efeb655b6380d60f5e64629fd1c26c35e 100644
--- a/VirtualRobot/IK/PoseQualityExtendedManipulability.h
+++ b/VirtualRobot/IK/PoseQualityExtendedManipulability.h
@@ -57,7 +57,7 @@ namespace VirtualRobot
PoseQualityExtendedManipulability(VirtualRobot::RobotNodeSetPtr rns, PoseQualityManipulability::ManipulabilityIndexType i = PoseQualityManipulability::eMinMaxRatio);
~PoseQualityExtendedManipulability();
- virtual float getPoseQuality();
+ float getPoseQuality() override;
float getPoseQuality(PoseQualityManipulability::ManipulabilityIndexType i, int considerFirstSV);
@@ -66,8 +66,8 @@ namespace VirtualRobot
The current configuration of the corresponding RNS is analyzed and the quality is returned.
\param direction A 3d or 6d vector with the Cartesian direction to investigate.
*/
- virtual float getPoseQuality(const Eigen::VectorXf& direction);
- virtual float getManipulability(const Eigen::VectorXf& direction, int considerFirstSV = -1);
+ float getPoseQuality(const Eigen::VectorXf& direction) override;
+ float getManipulability(const Eigen::VectorXf& direction, int considerFirstSV = -1) override;
struct extManipData
@@ -134,9 +134,9 @@ namespace VirtualRobot
static std::string getTypeName();
//! Indicates if joint limits are considered.
- virtual bool consideringJointLimits();
+ bool consideringJointLimits() override;
- virtual PoseQualityMeasurementPtr clone(RobotPtr newRobot);
+ PoseQualityMeasurementPtr clone(RobotPtr newRobot) override;
void getSelfDistParameters(float &storeAlpha, float &storeBeta);
diff --git a/VirtualRobot/IK/PoseQualityManipulability.h b/VirtualRobot/IK/PoseQualityManipulability.h
index 0b2a75067f190b43d441d0b954b899696931243e..5458b319be04845b5e27bbcee3770a4b9ee1ba5b 100644
--- a/VirtualRobot/IK/PoseQualityManipulability.h
+++ b/VirtualRobot/IK/PoseQualityManipulability.h
@@ -55,7 +55,7 @@ namespace VirtualRobot
/*!
Returns the manipulability of the current configuration.
*/
- virtual float getPoseQuality();
+ float getPoseQuality() override;
virtual float getManipulability(ManipulabilityIndexType i);
/*!
@@ -64,7 +64,7 @@ namespace VirtualRobot
See derived classes for details.
\param direction A 3d or 6d vector with the Cartesian direction to investigate.
*/
- virtual float getPoseQuality(const Eigen::VectorXf& direction);
+ float getPoseQuality(const Eigen::VectorXf& direction) override;
virtual float getManipulability(const Eigen::VectorXf& direction, int considerFirstSV = -1);
/*!
@@ -95,11 +95,11 @@ namespace VirtualRobot
*/
virtual void penalizeJointLimits(bool enable, float k = 50.0f);
- virtual bool consideringJointLimits();
+ bool consideringJointLimits() override;
static std::string getTypeName();
- virtual PoseQualityMeasurementPtr clone(RobotPtr newRobot);
+ PoseQualityMeasurementPtr clone(RobotPtr newRobot) override;
protected:
diff --git a/VirtualRobot/IK/constraints/BalanceConstraint.h b/VirtualRobot/IK/constraints/BalanceConstraint.h
index a479dc82d2f45831dc8cc4c3002386fb8182ab79..90b6a7087b85de5fcea8bc4f93a051762d9fd6cc 100644
--- a/VirtualRobot/IK/constraints/BalanceConstraint.h
+++ b/VirtualRobot/IK/constraints/BalanceConstraint.h
@@ -68,10 +68,10 @@ namespace VirtualRobot
BalanceConstraint(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const SupportPolygonPtr& supportPolygon,
float tolerance = 0.1f, float minimumStability = 0.5f, float maxSupportDistance = 10.0f, bool supportPolygonUpdates = true, bool considerCoMHeight = false);
- Eigen::MatrixXf getJacobianMatrix();
- Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
- Eigen::VectorXf getError(float stepSize = 1.0f);
- bool checkTolerances();
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override;
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
+ bool checkTolerances() override;
bool getRobotPoseForConstraint(RobotPtr& robot, Eigen::Matrix4f& pose);
Eigen::Vector3f getCoM();
@@ -79,8 +79,8 @@ namespace VirtualRobot
void setCoMHeight(float height);
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
protected:
void initialize(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const SceneObjectSetPtr& contactNodes,
diff --git a/VirtualRobot/IK/constraints/CoMConstraint.h b/VirtualRobot/IK/constraints/CoMConstraint.h
index 024b8427b8e9ce428dc49c820b698f5f103fdfea..fa8cc13f2d2bcc2afe72b5a908037f1b87a10442 100644
--- a/VirtualRobot/IK/constraints/CoMConstraint.h
+++ b/VirtualRobot/IK/constraints/CoMConstraint.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
CoMConstraint(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const Eigen::Vector3f& target, float tolerance);
CoMConstraint(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const Eigen::Vector2f& target, float tolerance);
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
- bool checkTolerances();
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
+ bool checkTolerances() override;
void updateTarget(const Eigen::Vector3f& target);
void updateTarget(const Eigen::Vector2f& target);
diff --git a/VirtualRobot/IK/constraints/OrientationConstraint.h b/VirtualRobot/IK/constraints/OrientationConstraint.h
index 10a1015449bfdacff9fb9e4b0f30606a86dd446f..0bd94060c0e37ee94588a149c7a555af418cf6dc 100644
--- a/VirtualRobot/IK/constraints/OrientationConstraint.h
+++ b/VirtualRobot/IK/constraints/OrientationConstraint.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
OrientationConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nodeSet, const SceneObjectPtr& eef, const Eigen::Matrix3f& target,
IKSolver::CartesianSelection cartesianSelection = IKSolver::All, float tolerance = 1.0f * M_PI / 180.0f, bool soft=false);
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
- bool checkTolerances();
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
+ bool checkTolerances() override;
protected:
RobotPtr robot;
diff --git a/VirtualRobot/IK/constraints/PoseConstraint.h b/VirtualRobot/IK/constraints/PoseConstraint.h
index fdca74a86c7f732cd77fbd5751f9486f3f3c96cb..2be5ea49907c1a06299064f04c5374bb443c7173 100644
--- a/VirtualRobot/IK/constraints/PoseConstraint.h
+++ b/VirtualRobot/IK/constraints/PoseConstraint.h
@@ -41,19 +41,19 @@ namespace VirtualRobot
void setVisualization(const SceneObjectSetPtr& visualizationNodeSet);
- Eigen::MatrixXf getJacobianMatrix();
- Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
- Eigen::VectorXf getError(float stepSize = 1.0f);
- bool checkTolerances();
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override;
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
+ bool checkTolerances() override;
- bool getRobotPoseForConstraint(Eigen::Matrix4f& pose);
+ bool getRobotPoseForConstraint(Eigen::Matrix4f& pose) override;
const Eigen::Matrix4f& getTarget();
void updateTarget(const Eigen::Matrix4f& newTarget);
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
protected:
double positionOptimizationFunction();
diff --git a/VirtualRobot/IK/constraints/PositionConstraint.h b/VirtualRobot/IK/constraints/PositionConstraint.h
index b3f712670758a181df9f22b0887f34992abdb2cf..60112b613bed0a3bb17391f30914989067166f03 100644
--- a/VirtualRobot/IK/constraints/PositionConstraint.h
+++ b/VirtualRobot/IK/constraints/PositionConstraint.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
PositionConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nodeSet, const SceneObjectPtr& eef, const Eigen::Vector3f& target,
IKSolver::CartesianSelection cartesianSelection = IKSolver::All, float tolerance = 3.0f);
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
- bool checkTolerances();
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
+ bool checkTolerances() override;
Eigen::Vector3f getTarget();
diff --git a/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h b/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h
index 83d2c23492b311fc9476882c98c430c9b0ae88f5..63202303acdb987b990ff149f746508dd96cd170 100644
--- a/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h
+++ b/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h
@@ -39,8 +39,8 @@ namespace VirtualRobot
void setReferenceConfiguration(const Eigen::VectorXf &config);
Eigen::VectorXf getReferenceConfiguration();
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
protected:
RobotPtr robot;
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.h b/VirtualRobot/IK/constraints/TSRConstraint.h
index 10d87dbdb85e5427f52b6f3a6c0b276a00cb7057..0dd565cf5b0c14579dda1aed20454b785eabf8dc 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.h
+++ b/VirtualRobot/IK/constraints/TSRConstraint.h
@@ -38,16 +38,16 @@ namespace VirtualRobot
const Eigen::Matrix4f& transformation, const Eigen::Matrix4f& eefOffset, const Eigen::Matrix<float, 6, 2>& bounds,
float tolerancePosition = 5.0f, float toleranceRotation = 3.0f / 180.0f * M_PI);
- Eigen::MatrixXf getJacobianMatrix();
- Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
- Eigen::VectorXf getError(float stepSize = 1.0f);
- bool checkTolerances();
+ Eigen::MatrixXf getJacobianMatrix() override;
+ Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp) override;
+ Eigen::VectorXf getError(float stepSize = 1.0f) override;
+ bool checkTolerances() override;
const Eigen::Matrix4f& getTransformation();
const Eigen::Matrix<float, 6, 2>& getBounds();
- double optimizationFunction(unsigned int id);
- Eigen::VectorXf optimizationGradient(unsigned int id);
+ double optimizationFunction(unsigned int id) override;
+ Eigen::VectorXf optimizationGradient(unsigned int id) override;
protected:
double positionOptimizationFunction();
diff --git a/VirtualRobot/Import/COLLADA-light/ColladaSimox.h b/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
index 3b5a819a318df1f15e65c2e67f737931d5c7a5bf..4b12b152fd840680ca66984c81bf53882e8935c5 100644
--- a/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
+++ b/VirtualRobot/Import/COLLADA-light/ColladaSimox.h
@@ -30,7 +30,7 @@ namespace Collada
ColladaSimoxRobotNode(VirtualRobot::RobotPtr simoxRobot, float scaleFactor) ;
~ColladaSimoxRobotNode();
- virtual void initialize();
+ void initialize() override;
VirtualRobot::RobotPtr simoxRobot;
VirtualRobot::RobotNodePtr simoxRobotNode;
float scaleFactor;
@@ -43,7 +43,7 @@ namespace Collada
float scaleFactor;
public:
ColladaSimoxRobot(float scaleFactor) ;
- virtual ColladaRobotNodePtr robotNodeFactory()
+ ColladaRobotNodePtr robotNodeFactory() override
{
return ColladaRobotNodePtr(new ColladaSimoxRobotNode(simoxRobot, scaleFactor));
}
diff --git a/VirtualRobot/Import/COLLADA-light/collada.cpp b/VirtualRobot/Import/COLLADA-light/collada.cpp
index 892819e50f2ee35694ba964deb4be9fc9d653029..b785b4a12aa1d417479e464d873f85a46595f2b8 100644
--- a/VirtualRobot/Import/COLLADA-light/collada.cpp
+++ b/VirtualRobot/Import/COLLADA-light/collada.cpp
@@ -124,7 +124,7 @@ namespace Collada
this->sensorMap = _sensorMap;
}
#endif
- bool for_each(pugi::xml_node& node)
+ bool for_each(pugi::xml_node& node) override
{
if (depth() + 1 > (int)stack.size())
{
diff --git a/VirtualRobot/Import/COLLADA-light/collada.h b/VirtualRobot/Import/COLLADA-light/collada.h
index f2df5beb7d8cca6cce8947985d95582729041663..0b9a0f23e67a2a4d558a7e107c460b43bde70624 100644
--- a/VirtualRobot/Import/COLLADA-light/collada.h
+++ b/VirtualRobot/Import/COLLADA-light/collada.h
@@ -57,7 +57,7 @@ namespace Collada
struct ColladaWalker : pugi::xml_tree_walker
{
ColladaWalker(StructureMap structureMap, XmlMap physicsMap) : structureMap(structureMap), physicsMap(physicsMap) {}
- virtual bool for_each(pugi::xml_node& node) = 0;
+ bool for_each(pugi::xml_node& node) override = 0;
StructureMap structureMap;
XmlMap physicsMap;
};
diff --git a/VirtualRobot/Import/COLLADA-light/inventor.h b/VirtualRobot/Import/COLLADA-light/inventor.h
index 738490ae5ca16bfcf2d65ea28388cbb995769b1d..9e86d29987d42a7d32a0add8c9143c47c353f2c5 100644
--- a/VirtualRobot/Import/COLLADA-light/inventor.h
+++ b/VirtualRobot/Import/COLLADA-light/inventor.h
@@ -16,7 +16,7 @@ namespace Collada
InventorRobotNode();
~InventorRobotNode();
void visualizeBoundingBox();
- virtual void initialize();
+ void initialize() override;
private:
bool m_bOwn;
};
@@ -27,7 +27,7 @@ namespace Collada
struct InventorWalker : ColladaWalker
{
InventorWalker(StructureMap _structureMap, XmlMap physicsMap, SoSeparator* _root) : ColladaWalker(_structureMap, physicsMap), root(_root) {}
- virtual bool for_each(pugi::xml_node& node);
+ bool for_each(pugi::xml_node& node) override;
SoSeparator* root;
std::vector<SoSeparator*> stack;
@@ -41,15 +41,15 @@ namespace Collada
private:
public:
InventorRobot(SoSeparator* _root) : root(_root) {}
- virtual ColladaRobotNodePtr robotNodeFactory()
+ ColladaRobotNodePtr robotNodeFactory() override
{
return ColladaRobotNodePtr(new InventorRobotNode(root));
}
- virtual ColladaWalkerPtr visualSceneWalkerFactory(StructureMap structureMap, XmlMap physicsMap)
+ ColladaWalkerPtr visualSceneWalkerFactory(StructureMap structureMap, XmlMap physicsMap) override
{
return ColladaWalkerPtr(new InventorWalker(structureMap, physicsMap, root));
}
- virtual void addCollisionModel(ColladaRobotNodePtr robotNode, pugi::xml_node shapeNode);
+ void addCollisionModel(ColladaRobotNodePtr robotNode, pugi::xml_node shapeNode) override;
protected:
SoSeparator* root;
InventorRobot() {};
diff --git a/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.hpp b/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.hpp
index 09bd6923dbc934bbd19b351841025eb93217bbec..562d89c8e5ddc3a6a87babc8cf56a559902e1463 100644
--- a/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.hpp
+++ b/VirtualRobot/Import/COLLADA-light/pugixml/pugixml.hpp
@@ -273,7 +273,7 @@ namespace pugi
// Construct writer from a FILE* object; void* is used to avoid header dependencies on stdio
xml_writer_file(void* file);
- virtual void write(const void* data, size_t size);
+ void write(const void* data, size_t size) override;
private:
void* file;
@@ -288,7 +288,7 @@ namespace pugi
xml_writer_stream(std::basic_ostream<char, std::char_traits<char> >& stream);
xml_writer_stream(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& stream);
- virtual void write(const void* data, size_t size);
+ void write(const void* data, size_t size) override;
private:
std::basic_ostream<char, std::char_traits<char> >* narrow_stream;
@@ -1214,7 +1214,7 @@ namespace pugi
explicit xpath_exception(const xpath_parse_result& result);
// Get error message
- virtual const char* what() const throw();
+ const char* what() const noexcept override;
// Get parse result
const xpath_parse_result& result() const;
diff --git a/VirtualRobot/Import/SimoxCOLLADAFactory.h b/VirtualRobot/Import/SimoxCOLLADAFactory.h
index 32ee8ab2d85fba085fe9e7ce32632b03201f5df5..3e893dcdda9b972c6bcea225131dffac4e2ec73d 100644
--- a/VirtualRobot/Import/SimoxCOLLADAFactory.h
+++ b/VirtualRobot/Import/SimoxCOLLADAFactory.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
{
public:
SimoxCOLLADAFactory();
- virtual ~SimoxCOLLADAFactory();
+ ~SimoxCOLLADAFactory() override;
- virtual RobotPtr loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode = RobotIO::eFull);
+ RobotPtr loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode = RobotIO::eFull) override;
// AbstractFactoryMethod
public:
@@ -52,8 +52,8 @@ namespace VirtualRobot
// RobotImporterFactory interface
public:
- virtual std::string getFileExtension();
- virtual std::string getFileFilter();
+ std::string getFileExtension() override;
+ std::string getFileFilter() override;
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/Import/SimoxXMLFactory.h b/VirtualRobot/Import/SimoxXMLFactory.h
index 11937897b18f7f96b3348b44315b3e55cd8d590c..cbcca0e2a4fe5f11f9edc14e5be7c5b451783e4f 100644
--- a/VirtualRobot/Import/SimoxXMLFactory.h
+++ b/VirtualRobot/Import/SimoxXMLFactory.h
@@ -38,9 +38,9 @@ namespace VirtualRobot
{
public:
SimoxXMLFactory();
- virtual ~SimoxXMLFactory();
+ ~SimoxXMLFactory() override;
- virtual RobotPtr loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode = RobotIO::eFull);
+ RobotPtr loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode = RobotIO::eFull) override;
// AbstractFactoryMethod
public:
@@ -52,8 +52,8 @@ namespace VirtualRobot
// RobotImporterFactory interface
public:
- virtual std::string getFileFilter();
- virtual std::string getFileExtension();
+ std::string getFileFilter() override;
+ std::string getFileExtension() override;
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/KinematicChain.h b/VirtualRobot/KinematicChain.h
index b545160f46a51dd40ee7fe81c413791fa0a4e213..4cebb315c09ccef268be084958dd4449508cf2bd 100644
--- a/VirtualRobot/KinematicChain.h
+++ b/VirtualRobot/KinematicChain.h
@@ -38,7 +38,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~KinematicChain();
+ ~KinematicChain() override;
private:
diff --git a/VirtualRobot/ManipulationObject.h b/VirtualRobot/ManipulationObject.h
index 20f5bc1497f124ef38adb31ca5e8620444d89160..794c2266b223523176f5a8f049ef75b54f41a014 100644
--- a/VirtualRobot/ManipulationObject.h
+++ b/VirtualRobot/ManipulationObject.h
@@ -42,9 +42,9 @@ namespace VirtualRobot
/*!
*/
- virtual ~ManipulationObject();
+ ~ManipulationObject() override;
- virtual void print(bool printDecoration = true);
+ void print(bool printDecoration = true) override;
bool hasGraspSet(GraspSetPtr graspSet);
bool hasGraspSet(const std::string& robotType, const std::string& eef);
diff --git a/VirtualRobot/Nodes/CameraSensor.h b/VirtualRobot/Nodes/CameraSensor.h
index 8389fb83d2ed3527ae6e1c34fbb3b39346f932dd..76c44a47d00372ccc55af579190b842ef243cf36 100644
--- a/VirtualRobot/Nodes/CameraSensor.h
+++ b/VirtualRobot/Nodes/CameraSensor.h
@@ -54,16 +54,16 @@ namespace VirtualRobot
/*!
*/
- virtual ~CameraSensor();
+ ~CameraSensor() override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual std::string toXML(const std::string& modelPath, int tabs);
+ std::string toXML(const std::string& modelPath, int tabs) override;
protected:
@@ -74,7 +74,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+ SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
};
diff --git a/VirtualRobot/Nodes/CameraSensorFactory.h b/VirtualRobot/Nodes/CameraSensorFactory.h
index 5b1dfc240e07b0580966435d98eb92bed94fc6cc..40629753e5e276d60361c68b51905423c2e9a028 100644
--- a/VirtualRobot/Nodes/CameraSensorFactory.h
+++ b/VirtualRobot/Nodes/CameraSensorFactory.h
@@ -38,16 +38,16 @@ namespace VirtualRobot
{
public:
CameraSensorFactory();
- virtual ~CameraSensorFactory();
+ ~CameraSensorFactory() override;
//! Standard init method
- virtual SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
- const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const;
+ SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- virtual SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const;
+ SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/ContactSensor.h b/VirtualRobot/Nodes/ContactSensor.h
index f3fd6c63f3c6b20c10d7647cd5df385310ccfdcc..f2e5341383efed3b3ca756c4581e974570c8f137 100644
--- a/VirtualRobot/Nodes/ContactSensor.h
+++ b/VirtualRobot/Nodes/ContactSensor.h
@@ -62,7 +62,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~ContactSensor();
+ ~ContactSensor() override;
/*!
* Set the new contact frame for this node.
@@ -88,10 +88,10 @@ namespace VirtualRobot
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual std::string toXML(const std::string& modelPath, int tabs);
+ std::string toXML(const std::string& modelPath, int tabs) override;
protected:
@@ -100,7 +100,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+ SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
ContactFrame frame;
double timestamp;
diff --git a/VirtualRobot/Nodes/ContactSensorFactory.h b/VirtualRobot/Nodes/ContactSensorFactory.h
index 33564ee108352e355dc435157c4f7b7851f823a9..e4f1d5c1bd4341b74e4484726b0616c98cf109a5 100644
--- a/VirtualRobot/Nodes/ContactSensorFactory.h
+++ b/VirtualRobot/Nodes/ContactSensorFactory.h
@@ -37,16 +37,16 @@ namespace VirtualRobot
{
public:
ContactSensorFactory();
- virtual ~ContactSensorFactory();
+ ~ContactSensorFactory() override;
//! Standard init method
- virtual SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
- const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const;
+ SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- virtual SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const;
+ SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.h b/VirtualRobot/Nodes/ForceTorqueSensor.h
index 36a865bc12ea9eb1cf7c4af8b2f962dda87804c4..c99cee18322cb9a1eb49995c24a4d7dff6cb93bd 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensor.h
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.h
@@ -55,7 +55,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~ForceTorqueSensor();
+ ~ForceTorqueSensor() override;
void updateSensors(const Eigen::VectorXf& newForceTorque);
@@ -72,10 +72,10 @@ namespace VirtualRobot
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual std::string toXML(const std::string& modelPath, int tabs);
+ std::string toXML(const std::string& modelPath, int tabs) override;
protected:
@@ -85,7 +85,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+ SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
Eigen::VectorXf forceTorqueValues;
};
diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.h b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
index 29ca9c2aa9ed3fa32796ed8ea7b29673e7f7d329..5e93c5da5c0902e12ec699aa7eb3ef1c2dd68456 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
+++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
@@ -36,16 +36,16 @@ namespace VirtualRobot
{
public:
ForceTorqueSensorFactory();
- virtual ~ForceTorqueSensorFactory();
+ ~ForceTorqueSensorFactory() override;
//! Standard init method
- virtual SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
- const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const;
+ SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- virtual SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const;
+ SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/PositionSensor.h b/VirtualRobot/Nodes/PositionSensor.h
index 5577075f9311d9dac6f67ce1582afbc2accbf9ff..31171afd77dff86d223e1285755f24bc2e532acf 100644
--- a/VirtualRobot/Nodes/PositionSensor.h
+++ b/VirtualRobot/Nodes/PositionSensor.h
@@ -55,16 +55,16 @@ namespace VirtualRobot
/*!
*/
- virtual ~PositionSensor();
+ ~PositionSensor() override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual std::string toXML(const std::string& modelPath, int tabs);
+ std::string toXML(const std::string& modelPath, int tabs) override;
protected:
@@ -74,7 +74,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling);
+ SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
};
diff --git a/VirtualRobot/Nodes/PositionSensorFactory.h b/VirtualRobot/Nodes/PositionSensorFactory.h
index 5d3c36a2dafe116fcfb42f0a16fdad980c25f703..beb338ef5991d429b8d86d8a4ac51d345e37c1d8 100644
--- a/VirtualRobot/Nodes/PositionSensorFactory.h
+++ b/VirtualRobot/Nodes/PositionSensorFactory.h
@@ -38,16 +38,16 @@ namespace VirtualRobot
{
public:
PositionSensorFactory();
- virtual ~PositionSensorFactory();
+ ~PositionSensorFactory() override;
//! Standard init method
- virtual SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
- const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const;
+ SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- virtual SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const;
+ SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index d7a35f286a927886677fae0085d4e3d26d25fef3..69233f59e217479221bc60391f41b42113376487 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -93,7 +93,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~RobotNode();
+ ~RobotNode() override;
RobotPtr getRobot() const;
@@ -137,17 +137,17 @@ namespace VirtualRobot
Be sure all children are created and registered to robot before calling initialize.
Usually RobotFactory manages the initialization.
*/
- virtual bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>());
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
/*!
Calling this method will cause an exception, since RobotNodes are controlled via joint values.
*/
- virtual void setGlobalPose(const Eigen::Matrix4f& pose);
+ void setGlobalPose(const Eigen::Matrix4f& pose) override;
/*
This call locks the robot's mutex.
*/
- virtual Eigen::Matrix4f getGlobalPose() const;
+ Eigen::Matrix4f getGlobalPose() const override;
/*!
The pose of this node in the root coordinate system of the robot.
@@ -172,13 +172,13 @@ namespace VirtualRobot
Then a visualziationNode has to be built and the \p visualizationType specifies which type of visualization should be used.
If not given, the first registered visaulizationfactory is used.
*/
- virtual void showCoordinateSystem(bool enable, float scaling = 1.0f, std::string* text = NULL, const std::string& visualizationType = "");
+ void showCoordinateSystem(bool enable, float scaling = 1.0f, std::string* text = NULL, const std::string& visualizationType = "") override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
float getJointLimitLo();
float getJointLimitHi();
@@ -300,7 +300,7 @@ namespace VirtualRobot
Compute/Update the transformations of this joint and all child joints. Therefore the parent is queried for its pose.
This method is called by the robot in order to update the pose matrices.
*/
- virtual void updatePose(bool updateChildren = true);
+ void updatePose(bool updateChildren = true) override;
/*!
@@ -346,7 +346,7 @@ namespace VirtualRobot
/*!
Update the pose according to parent
*/
- virtual void updatePose(const Eigen::Matrix4f& parentPose, bool updateChildren = true);
+ void updatePose(const Eigen::Matrix4f& parentPose, bool updateChildren = true) override;
/*!
@@ -393,7 +393,7 @@ namespace VirtualRobot
*/
virtual RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling) = 0;
- virtual SceneObject* _clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const
+ SceneObject* _clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const override
{
THROW_VR_EXCEPTION("Cloning not allowed this way...");
}
diff --git a/VirtualRobot/Nodes/RobotNodeFixed.h b/VirtualRobot/Nodes/RobotNodeFixed.h
index c2b65abba642d9bf005b0e4935d7713813de1e12..b7fc666bca90ff3a3be08258d7fe7a66a8c3c04b 100644
--- a/VirtualRobot/Nodes/RobotNodeFixed.h
+++ b/VirtualRobot/Nodes/RobotNodeFixed.h
@@ -73,26 +73,26 @@ namespace VirtualRobot
/*!
*/
- virtual ~RobotNodeFixed();
+ ~RobotNodeFixed() override;
- virtual bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>());
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
protected:
//! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
- virtual void checkValidRobotNodeType();
+ void checkValidRobotNodeType() override;
RobotNodeFixed() {};
- virtual void updateTransformationMatrices(const Eigen::Matrix4f& parentPose);
- virtual RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling);
+ void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
+ RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling) override;
/*!
Derived classes add custom XML tags here
*/
- virtual std::string _toXML(const std::string& modelPath);
+ std::string _toXML(const std::string& modelPath) override;
};
diff --git a/VirtualRobot/Nodes/RobotNodeFixedFactory.h b/VirtualRobot/Nodes/RobotNodeFixedFactory.h
index 42192981636630e5268515ddd99bbb5fe63a800d..4c8231c5c064743f5d0e68a35076aed92fb933a1 100644
--- a/VirtualRobot/Nodes/RobotNodeFixedFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeFixedFactory.h
@@ -38,10 +38,10 @@ namespace VirtualRobot
{
public:
RobotNodeFixedFactory();
- virtual ~RobotNodeFixedFactory();
+ ~RobotNodeFixedFactory() override;
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
+ RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.h b/VirtualRobot/Nodes/RobotNodePrismatic.h
index cfc4273e58adee6dc942d0239e90b7dbfa1fe7be..3a20a1e81ffedf55c64e66bda9749304740862ad 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.h
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.h
@@ -75,16 +75,16 @@ namespace VirtualRobot
RobotNodeType type = Generic);
/*!
*/
- virtual ~RobotNodePrismatic();
+ ~RobotNodePrismatic() override;
- virtual bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>());
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual bool isTranslationalJoint() const;
+ bool isTranslationalJoint() const override;
/*!
In global coord system.
@@ -113,21 +113,21 @@ namespace VirtualRobot
\param globalPose The new global pose. The joint value is *not* determined from this pose. The RobotNodeActuator is responsible for setting the corresponding joint value
\param updateChildren Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node (updateCHildren=true).
*/
- virtual void updateVisualizationPose(const Eigen::Matrix4f& globalPose, bool updateChildren = false);
+ void updateVisualizationPose(const Eigen::Matrix4f& globalPose, bool updateChildren = false) override;
//! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
- virtual void checkValidRobotNodeType();
+ void checkValidRobotNodeType() override;
RobotNodePrismatic() {};
- virtual void updateTransformationMatrices(const Eigen::Matrix4f& parentPose);
+ void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
Eigen::Vector3f jointTranslationDirection; // used when ePrismaticJoint (local coord system)
- virtual RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling);
+ RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling) override;
/*!
Derived classes add custom XML tags here
*/
- virtual std::string _toXML(const std::string& modelPath);
+ std::string _toXML(const std::string& modelPath) override;
bool visuScaling;
Eigen::Vector3f visuScaleFactor;
diff --git a/VirtualRobot/Nodes/RobotNodePrismaticFactory.h b/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
index 8d6a75027a35d2d028c4fa83e19fa228c8fcb508..d096318e3db5dd416445f327f7af7816a40a1547 100644
--- a/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
+++ b/VirtualRobot/Nodes/RobotNodePrismaticFactory.h
@@ -38,10 +38,10 @@ namespace VirtualRobot
{
public:
RobotNodePrismaticFactory();
- virtual ~RobotNodePrismaticFactory();
+ ~RobotNodePrismaticFactory() override;
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
+ RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/Nodes/RobotNodeRevolute.h b/VirtualRobot/Nodes/RobotNodeRevolute.h
index 192ecc2825c0e06cd3bdaa58dd3dc88f4a2720f3..3f0cc146977d7af205b5ff55fa48f872a85322b9 100644
--- a/VirtualRobot/Nodes/RobotNodeRevolute.h
+++ b/VirtualRobot/Nodes/RobotNodeRevolute.h
@@ -76,16 +76,16 @@ namespace VirtualRobot
RobotNodeType type = Generic);
/*!
*/
- virtual ~RobotNodeRevolute();
+ ~RobotNodeRevolute() override;
- virtual bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>());
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- virtual bool isRotationalJoint() const;
+ bool isRotationalJoint() const override;
/*!
Standard: In global coordinate system.
\param coordSystem When not set the axis is transformed to global coordinate system. Otherwise any scene object can be used as coord system.
@@ -121,22 +121,22 @@ namespace VirtualRobot
\param globalPose The new global pose. The joint value is *not* determined from this pose. The RobotNodeActuator is responsible for setting the corresponding joint value
\param updateChildren Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node (updateCHildren=true).
*/
- virtual void updateVisualizationPose(const Eigen::Matrix4f& globalPose, bool updateChildren = false);
+ void updateVisualizationPose(const Eigen::Matrix4f& globalPose, bool updateChildren = false) override;
//! Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
- virtual void checkValidRobotNodeType();
+ void checkValidRobotNodeType() override;
RobotNodeRevolute() {};
- virtual void updateTransformationMatrices(const Eigen::Matrix4f& parentPose);
+ void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
Eigen::Vector3f jointRotationAxis; // eRevoluteJoint (given in local joint coord system)
- virtual RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling);
+ RobotNodePtr _clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling) override;
/*!
Derived classes add custom XML tags here
*/
- virtual std::string _toXML(const std::string& modelPath);
+ std::string _toXML(const std::string& modelPath) override;
Eigen::Matrix4f tmpRotMat;
};
diff --git a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
index e5482aa57b5580679d3e1a1e62b3b87dcdf61e4a..6e8e634a001e6763f3bb92aa203de830918227f6 100644
--- a/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeRevoluteFactory.h
@@ -36,10 +36,10 @@ namespace VirtualRobot
{
public:
RobotNodeRevoluteFactory();
- virtual ~RobotNodeRevoluteFactory();
+ ~RobotNodeRevoluteFactory() override;
- virtual RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
- virtual RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const;
+ RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
+ RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
// AbstractFactoryMethod
public:
static std::string getName();
diff --git a/VirtualRobot/Nodes/Sensor.h b/VirtualRobot/Nodes/Sensor.h
index e2ec0e81c39327df83093781ea1b328b897979c1..1b30e9be5e3d79144ae5f287f275225f5384edbd 100644
--- a/VirtualRobot/Nodes/Sensor.h
+++ b/VirtualRobot/Nodes/Sensor.h
@@ -67,7 +67,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~Sensor();
+ ~Sensor() override;
RobotNodePtr getRobotNode() const;
@@ -89,12 +89,12 @@ namespace VirtualRobot
/*!
Calling this SceneObject method will cause an exception, since Sensors are controlled via their RobotNode parent.
*/
- virtual void setGlobalPose(const Eigen::Matrix4f& pose);
+ void setGlobalPose(const Eigen::Matrix4f& pose) override;
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
/*!
@@ -114,9 +114,9 @@ namespace VirtualRobot
/*!
Compute/Update the transformations of this sensor. Therefore the parent is queried for its pose.
*/
- virtual void updatePose(bool updateChildren = true);
+ void updatePose(bool updateChildren = true) override;
- virtual bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>());
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
virtual std::string toXML(const std::string& modelPath, int tabs = 1);
@@ -126,7 +126,7 @@ namespace VirtualRobot
/*!
Update the pose according to parent pose
*/
- virtual void updatePose(const Eigen::Matrix4f& parentPose, bool updateChildren = true);
+ void updatePose(const Eigen::Matrix4f& parentPose, bool updateChildren = true) override;
Sensor() {};
@@ -141,7 +141,7 @@ namespace VirtualRobot
*/
virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) = 0;
- virtual SceneObject* _clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const
+ SceneObject* _clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const override
{
THROW_VR_EXCEPTION("Cloning not allowed this way...");
}
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index 81934a2b56f9c7a268e87277cae02ae3732f8f6d..89a27f01410b2a8c69b1eb5e30e77c4ad6feefc4 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -47,7 +47,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~Obstacle();
+ ~Obstacle() override;
virtual void print(bool printDecoration = true);
@@ -104,7 +104,7 @@ namespace VirtualRobot
protected:
- virtual Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const;
+ Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const override;
std::string filename;
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index da67759d336a135ce83258fc6d06ed63d00af4ef..2f5a2838e1fe7f73267d50e61a1cb5a8a32356b9 100644
--- a/VirtualRobot/Primitive.h
+++ b/VirtualRobot/Primitive.h
@@ -37,7 +37,7 @@ namespace VirtualRobot
float width;
float height;
float depth;
- std::string toXMLString(int tabs = 0);
+ std::string toXMLString(int tabs = 0) override;
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Sphere : public Primitive
@@ -47,7 +47,7 @@ namespace VirtualRobot
Sphere() : Primitive(TYPE) {}
Sphere(float radius) : Primitive(TYPE), radius(radius) {}
float radius;
- std::string toXMLString(int tabs = 0);
+ std::string toXMLString(int tabs = 0) override;
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Cylinder : public Primitive
@@ -58,7 +58,7 @@ namespace VirtualRobot
Cylinder(float radius, float height) : Primitive(TYPE), radius(radius), height(height) {}
float radius;
float height;
- std::string toXMLString(int tabs = 0);
+ std::string toXMLString(int tabs = 0) override;
};
typedef boost::shared_ptr<Primitive> PrimitivePtr;
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index bd96e85958bf022c1b9cd5a825c3bb20e6f642b0..ba52cfa6f1b9fe596d1254fb6d9edd5026f8625c 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -64,7 +64,7 @@ namespace VirtualRobot
/*!
*/
- virtual ~Robot();
+ ~Robot() override;
/*!
The root node is the first RobotNode of this robot.
@@ -110,14 +110,14 @@ namespace VirtualRobot
\p enableInertial If true, a visualization of the inertial matrix is shown (if given).
\p comModel If set, this visualization is used to display the CoM location. If not set, a standard marker is used.
*/
- void showPhysicsInformation(bool enableCoM, bool enableInertial, VisualizationNodePtr comModel = VisualizationNodePtr());
+ void showPhysicsInformation(bool enableCoM, bool enableInertial, VisualizationNodePtr comModel = VisualizationNodePtr()) override;
/*!
Setup the full model visualization.
\param showVisualization If false, the visualization is disabled.
\param showAttachedVisualizations If false, the visualization of any attached optional visualizations is disabled.
*/
- void setupVisualization(bool showVisualization, bool showAttachedVisualizations);
+ void setupVisualization(bool showVisualization, bool showAttachedVisualizations) override;
virtual std::string getName();
@@ -126,13 +126,13 @@ namespace VirtualRobot
/*!
Print status information.
*/
- virtual void print(bool printChildren = false, bool printDecoration = true) const;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
/*!
Enables/Disables the visualization updates of collision model and visualization model.
*/
- void setUpdateVisualization(bool enable);
- void setUpdateCollisionModel(bool enable);
+ void setUpdateVisualization(bool enable) override;
+ void setUpdateCollisionModel(bool enable) override;
boost::shared_ptr<Robot> shared_from_this() const;
//boost::shared_ptr<Robot> shared_from_this() const { return boost::static_pointer_cast<Robot>(SceneObject::shared_from_this()); }
@@ -182,7 +182,7 @@ namespace VirtualRobot
virtual std::vector< CollisionModelPtr > getCollisionModels();
- virtual CollisionCheckerPtr getCollisionChecker();
+ CollisionCheckerPtr getCollisionChecker() override;
/*!
Convenient method for highlighting the visualization of this robot.
@@ -197,13 +197,13 @@ namespace VirtualRobot
get number of faces (i.e. triangles) of this object
\p collisionModel Indicates weather the faces of the collision model or the full model should be returned.
*/
- virtual int getNumFaces(bool collisionModel = false);
+ int getNumFaces(bool collisionModel = false) override;
/*!
Set the global position of this robot
*/
virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) = 0;
- void setGlobalPose(const Eigen::Matrix4f& globalPose);
+ void setGlobalPose(const Eigen::Matrix4f& globalPose) override;
/*!
Set the global pose of this robot so that the RobotNode node is at position globalPoseNode
@@ -215,11 +215,11 @@ namespace VirtualRobot
/*!
Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
*/
- virtual Eigen::Vector3f getCoMLocal();
+ Eigen::Vector3f getCoMLocal() override;
/*!
Return Center of Mass of this robot in global coordinates. All RobotNodes of this robot are considered according to their mass.
*/
- virtual Eigen::Vector3f getCoMGlobal();
+ Eigen::Vector3f getCoMGlobal() override;
/*!
Return accumulated mass of this robot.
@@ -468,37 +468,37 @@ namespace VirtualRobot
{
public:
LocalRobot(const std::string& name, const std::string& type = "");
- virtual ~LocalRobot();
+ ~LocalRobot() override;
- virtual void setRootNode(RobotNodePtr node);
- virtual RobotNodePtr getRootNode() const;
+ void setRootNode(RobotNodePtr node) override;
+ RobotNodePtr getRootNode() const override;
- virtual void registerRobotNode(RobotNodePtr node);
- virtual void deregisterRobotNode(RobotNodePtr node);
- virtual bool hasRobotNode(const std::string& robotNodeName);
- virtual bool hasRobotNode(RobotNodePtr node);
- virtual RobotNodePtr getRobotNode(const std::string& robotNodeName) const;
- virtual void getRobotNodes(std::vector< RobotNodePtr >& storeNodes, bool clearVector = true) const;
+ void registerRobotNode(RobotNodePtr node) override;
+ void deregisterRobotNode(RobotNodePtr node) override;
+ bool hasRobotNode(const std::string& robotNodeName) override;
+ bool hasRobotNode(RobotNodePtr node) override;
+ RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
+ void getRobotNodes(std::vector< RobotNodePtr >& storeNodes, bool clearVector = true) const override;
- virtual void registerRobotNodeSet(RobotNodeSetPtr nodeSet);
- virtual void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet);
- virtual bool hasRobotNodeSet(const std::string& name);
- virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const;
- virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const;
+ void registerRobotNodeSet(RobotNodeSetPtr nodeSet) override;
+ void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) override;
+ bool hasRobotNodeSet(const std::string& name) override;
+ RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
+ void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const override;
- virtual void registerEndEffector(EndEffectorPtr endEffector);
- virtual bool hasEndEffector(const std::string& endEffectorName);
- virtual EndEffectorPtr getEndEffector(const std::string& endEffectorName);
- virtual void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF);
+ void registerEndEffector(EndEffectorPtr endEffector) override;
+ bool hasEndEffector(const std::string& endEffectorName) override;
+ EndEffectorPtr getEndEffector(const std::string& endEffectorName) override;
+ void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) override;
- virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyJointValues = true);
- virtual void setGlobalPose(const Eigen::Matrix4f& globalPose);
- virtual Eigen::Matrix4f getGlobalPose() const;
+ void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyJointValues = true) override;
+ void setGlobalPose(const Eigen::Matrix4f& globalPose) override;
+ Eigen::Matrix4f getGlobalPose() const override;
protected:
//Eigen::Matrix4f globalPose; //!< The pose of this robot in the world
RobotNodePtr rootNode;
- virtual void applyJointValuesNoLock();
+ void applyJointValuesNoLock() override;
std::map< std::string, RobotNodePtr > robotNodeMap;
std::map< std::string, RobotNodeSetPtr > robotNodeSetMap;
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index c81892fa5e30998927346741eae8149c6e129066..6e2bf1e75a59008f7d1fc883310ad8935bcc9fd9 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -352,7 +352,7 @@ namespace VirtualRobot
typedef std::map < RobotNodePtr,
Eigen::Matrix4f,
std::less<RobotNodePtr>,
- Eigen::aligned_allocator<std::pair<const int, Eigen::Matrix4f> > >
+ Eigen::aligned_allocator<std::pair<const RobotNodePtr, Eigen::Matrix4f> > >
NodeTransformationMapT;
NodeTransformationMapT localTransformations;
diff --git a/VirtualRobot/RobotNodeSet.cpp b/VirtualRobot/RobotNodeSet.cpp
index ae0c3d7c6f886b63d94e462630cc12f82b3c9b55..480a8fac63614838422b18b84d138f85480c9067 100644
--- a/VirtualRobot/RobotNodeSet.cpp
+++ b/VirtualRobot/RobotNodeSet.cpp
@@ -24,7 +24,7 @@ namespace VirtualRobot
if (!kinematicRoot && robotNodes.size() > 0)
{
- THROW_VR_EXCEPTION_IF(rob, "RobotNodeSet::RobotNodeSet: Robot is NULL");
+ THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::RobotNodeSet: Robot is NULL");
this->kinematicRoot = rob->getRootNode();
}
@@ -35,7 +35,7 @@ namespace VirtualRobot
{
oldRootName = this->kinematicRoot->getName();
}
- THROW_VR_EXCEPTION_IF(rob, "RobotNodeSet::RobotNodeSet: Robot is NULL");
+ THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::RobotNodeSet: Robot is NULL");
this->kinematicRoot = rob->getRootNode();
if(this->kinematicRoot)
{
@@ -415,8 +415,7 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
RobotPtr rob = robot.lock();
- // commented out to avoid exception in headIK in armarx --> works anyway!?
-// THROW_VR_EXCEPTION_IF(rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
+ THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i = 0; i < robotNodes.size(); i++)
@@ -439,8 +438,7 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(static_cast<std::size_t>(jointValues.rows()) != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
RobotPtr rob = robot.lock();
- // commented out to avoid exception in headIK in armarx --> works anyway!?
-// THROW_VR_EXCEPTION_IF(rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
+ THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i = 0; i < robotNodes.size(); i++)
@@ -462,8 +460,7 @@ namespace VirtualRobot
{
VR_ASSERT(jointValues);
RobotPtr rob = robot.lock();
- // commented out to avoid exception in headIK in armarx --> works anyway!?
-// THROW_VR_EXCEPTION_IF(rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
+ THROW_VR_EXCEPTION_IF(!rob, "RobotNodeSet::setJointValues: Robot is NULL (The last shared_ptr was deleted)");
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i = 0; i < robotNodes.size(); i++)
diff --git a/VirtualRobot/RobotNodeSet.h b/VirtualRobot/RobotNodeSet.h
index 6f884b18bb5a9fc821009e2e5098b49207f07244..c5a8e0375bbe131e542c23acc63151963033e231 100644
--- a/VirtualRobot/RobotNodeSet.h
+++ b/VirtualRobot/RobotNodeSet.h
@@ -89,7 +89,7 @@ namespace VirtualRobot
/*!
The number of associated robot nodes.
*/
- virtual unsigned int getSize() const;
+ unsigned int getSize() const override;
std::vector<float> getJointValues() const;
Eigen::VectorXf getJointValuesEigen() const;
@@ -191,18 +191,18 @@ namespace VirtualRobot
bool nodesSufficient(std::vector<RobotNodePtr> nodes) const;
- virtual std::string toXML(int tabs);
+ std::string toXML(int tabs) override;
//! this is forbidden for RobotNodeSets, a call will throw an exception
- virtual bool addSceneObject(SceneObjectPtr sceneObject);
+ bool addSceneObject(SceneObjectPtr sceneObject) override;
//! this is forbidden for RobotNodeSets, a call will throw an exception
- virtual bool addSceneObjects(SceneObjectSetPtr sceneObjectSet);
+ bool addSceneObjects(SceneObjectSetPtr sceneObjectSet) override;
//! this is forbidden for RobotNodeSets, a call will throw an exception
- virtual bool addSceneObjects(RobotNodeSetPtr robotNodeSet);
+ bool addSceneObjects(RobotNodeSetPtr robotNodeSet) override;
//! this is forbidden for RobotNodeSets, a call will throw an exception
- virtual bool addSceneObjects(std::vector<RobotNodePtr> robotNodes);
+ bool addSceneObjects(std::vector<RobotNodePtr> robotNodes) override;
//! this is forbidden for RobotNodeSets, a call will throw an exception
- virtual bool removeSceneObject(SceneObjectPtr sceneObject);
+ bool removeSceneObject(SceneObjectPtr sceneObject) override;
protected:
/*!
diff --git a/VirtualRobot/TimeOptimalTrajectory/Path.cpp b/VirtualRobot/TimeOptimalTrajectory/Path.cpp
index c0f179090065307fbcdddf552bd2f89a1af2dc77..d6398d49630e176d239dd6f13c0a915b3483301b 100644
--- a/VirtualRobot/TimeOptimalTrajectory/Path.cpp
+++ b/VirtualRobot/TimeOptimalTrajectory/Path.cpp
@@ -59,25 +59,25 @@ namespace VirtualRobot
{
}
- Eigen::VectorXd getConfig(double s) const {
+ Eigen::VectorXd getConfig(double s) const override {
s /= length;
s = std::max(0.0, std::min(1.0, s));
return (1.0 - s) * start + s * end;
}
- Eigen::VectorXd getTangent(double /* s */) const {
+ Eigen::VectorXd getTangent(double /* s */) const override {
return (end - start) / length;
}
- Eigen::VectorXd getCurvature(double /* s */) const {
+ Eigen::VectorXd getCurvature(double /* s */) const override {
return Eigen::VectorXd::Zero(start.size());
}
- list<double> getSwitchingPoints() const {
+ list<double> getSwitchingPoints() const override {
return list<double>();
}
- LinearPathSegment* clone() const {
+ LinearPathSegment* clone() const override {
return new LinearPathSegment(*this);
}
@@ -128,22 +128,22 @@ namespace VirtualRobot
y = startDirection;
}
- Eigen::VectorXd getConfig(double s) const {
+ Eigen::VectorXd getConfig(double s) const override {
const double angle = s / radius;
return center + radius * (x * cos(angle) + y * sin(angle));
}
- Eigen::VectorXd getTangent(double s) const {
+ Eigen::VectorXd getTangent(double s) const override {
const double angle = s / radius;
return - x * sin(angle) + y * cos(angle);
}
- Eigen::VectorXd getCurvature(double s) const {
+ Eigen::VectorXd getCurvature(double s) const override {
const double angle = s / radius;
return - 1.0 / radius * (x * cos(angle) + y * sin(angle));
}
- list<double> getSwitchingPoints() const {
+ list<double> getSwitchingPoints() const override {
list<double> switchingPoints;
const double dim = x.size();
for(unsigned int i = 0; i < dim; i++) {
@@ -160,7 +160,7 @@ namespace VirtualRobot
return switchingPoints;
}
- CircularPathSegment* clone() const {
+ CircularPathSegment* clone() const override {
return new CircularPathSegment(*this);
}
diff --git a/VirtualRobot/VirtualRobotException.h b/VirtualRobot/VirtualRobotException.h
index 4a4610b7a908e9006d3ced223f84d57bc069bb7c..0160d01266340537cb10aa798e63ad0a821b6ff0 100644
--- a/VirtualRobot/VirtualRobotException.h
+++ b/VirtualRobot/VirtualRobotException.h
@@ -46,9 +46,9 @@ namespace VirtualRobot
VirtualRobotException(const std::string& what);
VirtualRobotException(const char* what);
- virtual ~VirtualRobotException() throw();
+ ~VirtualRobotException() noexcept override;
- virtual const char* what() const throw();
+ const char* what() const noexcept override;
//virtual const char * what() const;
protected:
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
index 884ecb80776d8683228316e24f951705f6e5959a..9728a7c4a9d0fb713fa0686a8f1b9187c41c9e4a 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h
@@ -44,21 +44,21 @@ namespace VirtualRobot
CoinVisualization(const VisualizationNodePtr visualizationNode);
CoinVisualization(const std::vector<VisualizationNodePtr>& visualizationNodes);
- virtual ~CoinVisualization();
+ ~CoinVisualization() override;
/*!
To see the visualizations in an SoExaminerViewer enable an highlight render action
e.g. viewer->setGLRenderAction(new SoLineHighlightRenderAction);
*/
- virtual bool highlight(VisualizationNodePtr visualizationNode, bool enable);
- virtual bool highlight(unsigned int which, bool enable);
+ bool highlight(VisualizationNodePtr visualizationNode, bool enable) override;
+ bool highlight(unsigned int which, bool enable) override;
virtual bool highlight(bool enable);
virtual bool highlight(SoNode* visu, bool enable);
- virtual void colorize(VisualizationFactory::Color c);
- virtual void setTransparency(float transparency);
+ void colorize(VisualizationFactory::Color c) override;
+ void setTransparency(float transparency) override;
- virtual VisualizationPtr clone();
+ VisualizationPtr clone() override;
SoNode* getCoinVisualization(bool selectable=true);
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index da041f909d65e0d14cda0f74c350cfda88a13c08..77b3b398d9c8dd97e185f3115811fdbe229d873e 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -1094,7 +1094,7 @@ namespace VirtualRobot
// SoDataSensor interface
public:
- void dyingReference()
+ void dyingReference() override
{
boost::mutex::scoped_lock lock(CoinVisualizationFactory::globalTextureCacheMutex);
CoinVisualizationFactory::globalTextureCache.erase(std::make_pair(filesize, path));
@@ -1102,7 +1102,7 @@ namespace VirtualRobot
}
private:
- ~DeleteTextureCallBack(){}
+ ~DeleteTextureCallBack() override{}
std::string nodeName;
std::string path;
size_t filesize;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 134ae793a34bad16907d10ea9b24de30564d26eb..12a78162ef73fa33e9748ec3c75aa222c63ea77e 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -57,20 +57,20 @@ namespace VirtualRobot
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CoinVisualizationFactory();
- virtual ~CoinVisualizationFactory();
+ ~CoinVisualizationFactory() override;
/*!
Initialises SoDB and SoQt.
Sets the COIN_SEPARATE_DIFFUSE_TRANSPARENCY_OVERRIDE environment variable to enable a Coin3D transparency extension.
*/
- virtual void init(int &argc, char* argv[], const std::string &appName);
+ void init(int &argc, char* argv[], const std::string &appName) override;
- virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr>& primitives, bool boundingBox = false, Color color = Color::Gray());
- virtual VisualizationNodePtr getVisualizationFromFile(const std::string& filename, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f);
+ VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr>& primitives, bool boundingBox = false, Color color = Color::Gray()) override;
+ VisualizationNodePtr getVisualizationFromFile(const std::string& filename, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override;
virtual VisualizationNodePtr getVisualizationFromSTLFile(const std::string& filename, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f);
virtual VisualizationNodePtr getVisualizationFromCoin3DFile(const std::string& filename, bool boundingBox = false);
- virtual VisualizationNodePtr getVisualizationFromFile(const std::ifstream& ifs, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f);
+ VisualizationNodePtr getVisualizationFromFile(const std::ifstream& ifs, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override;
virtual VisualizationNodePtr getVisualizationFromString(const std::string& modelString, bool boundingBox = false);
@@ -79,23 +79,23 @@ namespace VirtualRobot
virtual VisualizationPtr getVisualization(VisualizationNodePtr visu);
- virtual VisualizationNodePtr createBox(float width, float height, float depth, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createLine(const Eigen::Vector3f& from, const Eigen::Vector3f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createLine(const Eigen::Matrix4f& from, const Eigen::Matrix4f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createSphere(float radius, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createCylinder(float radius, float height, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createCircle(float radius, float circleCompletion, float width, float colorR = 1.0f, float colorG = 0.5f, float colorB = 0.5f, size_t numberOfCircleParts = 30);
- virtual VisualizationNodePtr createCoordSystem(float scaling = 1.0f, std::string* text = NULL, float axisLength = 100.0f, float axisSize = 3.0f, int nrOfBlocks = 10);
- virtual VisualizationNodePtr createBoundingBox(const BoundingBox& bbox, bool wireFrame = false);
- virtual VisualizationNodePtr createVertexVisualization(const Eigen::Vector3f& position, float radius, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, Eigen::Matrix4f& pose, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f);
- virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, bool showNormals, Eigen::Matrix4f& pose, bool showLines = true);
- virtual VisualizationNodePtr createPlane(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f);
- virtual VisualizationNodePtr createArrow(const Eigen::Vector3f& n, float length = 50.0f, float width = 2.0f, const Color& color = Color::Gray());
- virtual VisualizationNodePtr createCircleArrow(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30);
- virtual VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f);
- virtual VisualizationNodePtr createText(const std::string& text, bool billboard = false, float scaling = 1.0f, Color c = Color::Black(), float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f);
- virtual VisualizationNodePtr createTorus(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30);
+ VisualizationNodePtr createBox(float width, float height, float depth, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createLine(const Eigen::Vector3f& from, const Eigen::Vector3f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createLine(const Eigen::Matrix4f& from, const Eigen::Matrix4f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createSphere(float radius, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createCylinder(float radius, float height, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createCircle(float radius, float circleCompletion, float width, float colorR = 1.0f, float colorG = 0.5f, float colorB = 0.5f, size_t numberOfCircleParts = 30) override;
+ VisualizationNodePtr createCoordSystem(float scaling = 1.0f, std::string* text = NULL, float axisLength = 100.0f, float axisSize = 3.0f, int nrOfBlocks = 10) override;
+ VisualizationNodePtr createBoundingBox(const BoundingBox& bbox, bool wireFrame = false) override;
+ VisualizationNodePtr createVertexVisualization(const Eigen::Vector3f& position, float radius, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, Eigen::Matrix4f& pose, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override;
+ VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr model, bool showNormals, Eigen::Matrix4f& pose, bool showLines = true) override;
+ VisualizationNodePtr createPlane(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override;
+ VisualizationNodePtr createArrow(const Eigen::Vector3f& n, float length = 50.0f, float width = 2.0f, const Color& color = Color::Gray()) override;
+ VisualizationNodePtr createCircleArrow(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30) override;
+ VisualizationNodePtr createTrajectory(TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f) override;
+ VisualizationNodePtr createText(const std::string& text, bool billboard = false, float scaling = 1.0f, Color c = Color::Black(), float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f) override;
+ VisualizationNodePtr createTorus(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30) override;
/*!
Creates an coordinate axis aligned ellipse
\param x The extend in x direction must be >= 1e-6
@@ -106,22 +106,22 @@ namespace VirtualRobot
\param axesWidth The width of the axes.
\return A VisualizationNode containing the visualization.
*/
- virtual VisualizationNodePtr createEllipse(float x, float y, float z, bool showAxes = true, float axesHeight = 4.0f, float axesWidth = 8.0f);
+ VisualizationNodePtr createEllipse(float x, float y, float z, bool showAxes = true, float axesHeight = 4.0f, float axesWidth = 8.0f) override;
/*!
Move local visualization by homogeneous matrix m. MM is used.
*/
- virtual void applyDisplacement(VisualizationNodePtr o, Eigen::Matrix4f& m);
+ void applyDisplacement(VisualizationNodePtr o, Eigen::Matrix4f& m) override;
/*!
Create an empty VisualizationNode.
*/
- virtual VisualizationNodePtr createVisualization();
+ VisualizationNodePtr createVisualization() override;
/*!
Create a united visualization. Internally all visualizations are copied and added to one SoSeparator.
All visualizations have to be of type CoinVisualizationNode.
*/
- virtual VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr>& visualizations) const;
+ VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr>& visualizations) const override;
static SoSeparator* CreateConvexHull2DVisualization(const MathTools::ConvexHull2DPtr ch, MathTools::Plane& p, VisualizationFactory::Color colorInner = VisualizationFactory::Color::Blue(), VisualizationFactory::Color colorLine = VisualizationFactory::Color::Black(), float lineSize = 5.0f, const Eigen::Vector3f& offset = Eigen::Vector3f::Zero());
@@ -427,7 +427,7 @@ namespace VirtualRobot
Here, a manual cleanup can be called, no Coin3D access possible after this.
Usually no need to call cleanup explicitly, since cleanup is performed automatically at application exit.
*/
- virtual void cleanup();
+ void cleanup() override;
protected:
static SoNode* GetNodeFromPrimitive(Primitive::PrimitivePtr primitive, bool boundingBox, Color color);
static void GetVisualizationFromSoInput(SoInput& soInput, VisualizationNodePtr& visualizationNode, bool bbox = false, bool freeDuplicateTextures = true);
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
index b8485b596922d93357b02bd63abe36f961d31fd7..acf844de2ce9e473ec487a4f53102b928c5b1513 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h
@@ -45,34 +45,34 @@ namespace VirtualRobot
friend class CoinVisualizationFactory;
public:
CoinVisualizationNode(SoNode* visualizationNode, float margin = 0.0f);
- ~CoinVisualizationNode();
- virtual TriMeshModelPtr getTriMeshModel();
+ ~CoinVisualizationNode() override;
+ TriMeshModelPtr getTriMeshModel() override;
SoNode* getCoinVisualization();
- virtual void setGlobalPose(const Eigen::Matrix4f& m);
+ void setGlobalPose(const Eigen::Matrix4f& m) override;
- virtual void print();
+ void print() override;
- virtual void scale(Eigen::Vector3f& scaleFactor);
+ void scale(Eigen::Vector3f& scaleFactor) override;
/*!
Attach an optional visualization to this VisualizationNode. The attached visualizations will not show up in the TriMeshModel.
If there is already a visualization attached with the given name, it is quietly replaced.
*/
- virtual void attachVisualization(const std::string& name, VisualizationNodePtr v);
+ void attachVisualization(const std::string& name, VisualizationNodePtr v) override;
/*!
Remove an attached visualization.
*/
- virtual void detachVisualization(const std::string& name);
+ void detachVisualization(const std::string& name) override;
/*!
Setup the visualization of this object.
\param showVisualization If false, the visualization is disabled.
\param showAttachedVisualizations If false, the visualization of any attached optional visualizations is disabled.
*/
- virtual void setupVisualization(bool showVisualization, bool showAttachedVisualizations);
+ void setupVisualization(bool showVisualization, bool showAttachedVisualizations) override;
/*!
@@ -81,9 +81,9 @@ namespace VirtualRobot
\param scaling Scale Can be set to create a scaled version of this visual data.
Since the underlying implementation may be able to re-use the visualization data, a deep copy may not be necessary in some cases.
*/
- virtual VisualizationNodePtr clone(bool deepCopy = true, float scaling = 1.0f);
+ VisualizationNodePtr clone(bool deepCopy = true, float scaling = 1.0f) override;
- virtual std::string getType()
+ std::string getType() override
{
return CoinVisualizationFactory::getName();
}
@@ -93,9 +93,9 @@ namespace VirtualRobot
\param modelPath The directory.
\param filename The new filename. If filename extension is ".iv", the file is stored in Open Inventor format. Otherwise the file is stored in VRML2 format (.wrl).
*/
- virtual bool saveModel(const std::string& modelPath, const std::string& filename);
- virtual void shrinkFatten(float offset);
- virtual void createTriMeshModel();
+ bool saveModel(const std::string& modelPath, const std::string& filename) override;
+ void shrinkFatten(float offset) override;
+ void createTriMeshModel() override;
protected:
diff --git a/VirtualRobot/Workspace/Manipulability.h b/VirtualRobot/Workspace/Manipulability.h
index 676516d86b71ac1f4e7ee755d480c2c3dd1896d7..f057cfec55f4b04d8c39aca6a920a1f46a1edc48 100644
--- a/VirtualRobot/Workspace/Manipulability.h
+++ b/VirtualRobot/Workspace/Manipulability.h
@@ -163,7 +163,7 @@ namespace VirtualRobot
/*!
Creates a deep copy of this data structure. A ManipulabilityPtr is returned.
*/
- virtual WorkspaceRepresentationPtr clone();
+ WorkspaceRepresentationPtr clone() override;
/*!
Appends a number of random TCP poses to workspace Data (multithreaded).
@@ -172,21 +172,21 @@ namespace VirtualRobot
\param numThreads number of worker threads used behind the scenes to append random TCP poses to workspace data.
\param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free.
*/
- virtual void addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions = true);
+ void addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions = true) override;
protected:
- virtual bool customLoad(std::ifstream& file);
- virtual bool customSave(std::ofstream& file);
- virtual void customPrint();
+ bool customLoad(std::ifstream& file) override;
+ bool customSave(std::ofstream& file) override;
+ void customPrint() override;
- virtual void customInitialize();
+ void customInitialize() override;
bool customStringRead(std::ifstream& file, std::string& res);
float getCurrentManipulability(PoseQualityMeasurementPtr qualMeasure, RobotNodeSetPtr selfDistSt = RobotNodeSetPtr(), RobotNodeSetPtr selfDistDyn = RobotNodeSetPtr());
- void addPose(const Eigen::Matrix4f& p);
- void addPose(const Eigen::Matrix4f& p, PoseQualityMeasurementPtr qualMeasure);
+ void addPose(const Eigen::Matrix4f& p) override;
+ void addPose(const Eigen::Matrix4f& p, PoseQualityMeasurementPtr qualMeasure) override;
void addPose(const Eigen::Matrix4f& p, PoseQualityMeasurementPtr qualMeasure, RobotNodeSetPtr selfDistSt, RobotNodeSetPtr selfDistDyn);
PoseQualityMeasurementPtr measure;
diff --git a/VirtualRobot/Workspace/Reachability.h b/VirtualRobot/Workspace/Reachability.h
index 8c9e2dacffb9379e188c7778916bbd21c0faa7c2..8d8b1e85163280f05b427e84adb997645df3636a 100644
--- a/VirtualRobot/Workspace/Reachability.h
+++ b/VirtualRobot/Workspace/Reachability.h
@@ -68,7 +68,7 @@ namespace VirtualRobot
/*!
Creates a deep copy of this data structure. A ReachabilityPtr is returned.
*/
- virtual WorkspaceRepresentationPtr clone();
+ WorkspaceRepresentationPtr clone() override;
protected:
diff --git a/VirtualRobot/Workspace/WorkspaceDataArray.h b/VirtualRobot/Workspace/WorkspaceDataArray.h
index ad64311d16d23d9e087d53dd32b48b08f0a26984..ed86c5eeb90467b5df5843c3b8e59ad04bf50682 100644
--- a/VirtualRobot/Workspace/WorkspaceDataArray.h
+++ b/VirtualRobot/Workspace/WorkspaceDataArray.h
@@ -58,22 +58,22 @@ namespace VirtualRobot
//! create Workspace out of file
WorkspaceDataArray(std::ofstream& file);
- ~WorkspaceDataArray();
+ ~WorkspaceDataArray() override;
//! Return the amount of data in bytes
- unsigned int getSizeTr() const;
- unsigned int getSizeRot() const;
+ unsigned int getSizeTr() const override;
+ unsigned int getSizeRot() const override;
- void setDatum(float x[], unsigned char value, const WorkspaceRepresentation* workspace);
+ void setDatum(float x[], unsigned char value, const WorkspaceRepresentation* workspace) override;
void setDatum(unsigned int x0, unsigned int x1, unsigned int x2,
- unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value);
+ unsigned int x3, unsigned int x4, unsigned int x5, unsigned char value) override;
- inline void setDatum(unsigned int x[6], unsigned char value);
+ inline void setDatum(unsigned int x[6], unsigned char value) override;
- void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels);
+ void setDatumCheckNeighbors(unsigned int x[6], unsigned char value, unsigned int neighborVoxels) override;
- void increaseDatum(float x[], const WorkspaceRepresentation* workspace);
+ void increaseDatum(float x[], const WorkspaceRepresentation* workspace) override;
inline void increaseDatum(unsigned int x0, unsigned int x1, unsigned int x2,
unsigned int x3, unsigned int x4, unsigned int x5);
@@ -82,44 +82,44 @@ namespace VirtualRobot
/*!
Set rotation data for given x,y,z position.
*/
- void setDataRot(unsigned char* data, unsigned int x, unsigned int y, unsigned int z);
+ void setDataRot(unsigned char* data, unsigned int x, unsigned int y, unsigned int z) override;
/*!
Get rotation data for given x,y,z position.
*/
- const unsigned char* getDataRot(unsigned int x, unsigned int y, unsigned int z);
+ const unsigned char* getDataRot(unsigned int x, unsigned int y, unsigned int z) override;
- unsigned char get(float x[], const WorkspaceRepresentation* workspace);
+ unsigned char get(float x[], const WorkspaceRepresentation* workspace) override;
int getMaxSummedAngleReachablity();
//! 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);
+ unsigned int x3, unsigned int x4, unsigned int x5) override;
//! Simulates a multi-dimensional array access
- inline unsigned char get(unsigned int x[6]);
+ inline unsigned char get(unsigned int x[6]) override;
- bool hasEntry(unsigned int x, unsigned int y, unsigned int z);
+ bool hasEntry(unsigned int x, unsigned int y, unsigned int z) override;
// Set all entries to 0
- void clear();
- void binarize();
+ void clear() override;
+ void binarize() override;
- void bisectData();
+ void bisectData() override;
- unsigned int getSize(int dim)
+ unsigned int getSize(int dim) override
{
return sizes[dim];
}
- unsigned char** getRawData()
+ unsigned char** getRawData() override
{
return data;
}
- WorkspaceData* clone();
+ WorkspaceData* clone() override;
- bool save(std::ofstream& file);
+ bool save(std::ofstream& file) override;
protected:
void ensureData(unsigned int x, unsigned int y, unsigned int z);
diff --git a/VirtualRobot/XML/ObjectIO.h b/VirtualRobot/XML/ObjectIO.h
index 14d33a191aa70989c48cc166d96b800cfe68f60d..ae3726ad62b156d65cc5a2e484286b7c42df8f43 100644
--- a/VirtualRobot/XML/ObjectIO.h
+++ b/VirtualRobot/XML/ObjectIO.h
@@ -114,7 +114,7 @@ namespace VirtualRobot
// instantiation not allowed
ObjectIO();
- virtual ~ObjectIO();
+ ~ObjectIO() override;
static bool processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath);
};
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index 86e14af745bff607724d9be59cec309f5010d5c0..beae1342538bba122a3112ebf6ffdcefd34ea124 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -95,7 +95,7 @@ namespace VirtualRobot
// instantiation not allowed
RobotIO();
- virtual ~RobotIO();
+ ~RobotIO() override;
static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode = eFull);
static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot);
diff --git a/VirtualRobot/XML/SceneIO.h b/VirtualRobot/XML/SceneIO.h
index 7040d965fdbed13a268d89ccbf935e1f9ae89b02..69e14e4449157ee3fdaee05160bfecfda823b9f1 100644
--- a/VirtualRobot/XML/SceneIO.h
+++ b/VirtualRobot/XML/SceneIO.h
@@ -67,7 +67,7 @@ namespace VirtualRobot
// instantiation not allowed
SceneIO();
- virtual ~SceneIO();
+ ~SceneIO() override;
static ScenePtr processScene(rapidxml::xml_node<char>* sceneXMLNode, const std::string& basePath);
static ScenePtr processSceneAttributes(rapidxml::xml_node<char>* sceneXMLNode);
static bool processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath);
diff --git a/VirtualRobot/XML/rapidxml.hpp b/VirtualRobot/XML/rapidxml.hpp
index 0cfa90f11a16b5df69d73a61721396f72c291233..6e63226162a96f16460c4c9f670c5581f71486c2 100644
--- a/VirtualRobot/XML/rapidxml.hpp
+++ b/VirtualRobot/XML/rapidxml.hpp
@@ -82,7 +82,7 @@ namespace rapidxml
//! Gets human readable description of error.
//! \return Pointer to null terminated description of the error.
- virtual const char* what() const throw()
+ const char* what() const throw() override
{
return m_what;
}
diff --git a/VirtualRobot/examples/CameraViewer/showCamWindow.h b/VirtualRobot/examples/CameraViewer/showCamWindow.h
index 7bdb9c69d3167a626dd562299ad1bf1d769bbc39..509e7fe21ca69d992a8c55e507291fe79932b7d9 100644
--- a/VirtualRobot/examples/CameraViewer/showCamWindow.h
+++ b/VirtualRobot/examples/CameraViewer/showCamWindow.h
@@ -35,7 +35,7 @@ class showCamWindow : public QMainWindow
Q_OBJECT
public:
showCamWindow(std::string& sRobotFilename, std::string& cam1Name, std::string& cam2Name);
- ~showCamWindow();
+ ~showCamWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -45,7 +45,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void rebuildVisualization();
diff --git a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.h b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.h
index 0f9952640eac181e7690cb7eadcc4b779e7cf55a..d56f878d53cfcfaaff9cce81be2087a2bbc1544f 100644
--- a/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.h
+++ b/VirtualRobot/examples/ConstrainedIK/ConstrainedIKWindow.h
@@ -63,7 +63,7 @@ class ConstrainedIKWindow : public QMainWindow
Q_OBJECT
public:
ConstrainedIKWindow(std::string& sRobotFilename);
- ~ConstrainedIKWindow();
+ ~ConstrainedIKWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -73,7 +73,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void collisionModel();
diff --git a/VirtualRobot/examples/GenericIK/GenericIKWindow.h b/VirtualRobot/examples/GenericIK/GenericIKWindow.h
index 5fa8d651be062b0a09358caa1e687fe9ffc295f3..82a74b7c0637ddd621b38706d5d3cf7554faeffe 100644
--- a/VirtualRobot/examples/GenericIK/GenericIKWindow.h
+++ b/VirtualRobot/examples/GenericIK/GenericIKWindow.h
@@ -34,7 +34,7 @@ class GenericIKWindow : public QMainWindow
Q_OBJECT
public:
GenericIKWindow(std::string& sRobotFilename);
- ~GenericIKWindow();
+ ~GenericIKWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -44,7 +44,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void collisionModel();
diff --git a/VirtualRobot/examples/Jacobi/JacobiWindow.h b/VirtualRobot/examples/Jacobi/JacobiWindow.h
index 1971a2dc0e31fac51a8d6cee23f25d3fc616f2b1..5b60675643ca3019dd22cbcd6fc68756f816fbfe 100644
--- a/VirtualRobot/examples/Jacobi/JacobiWindow.h
+++ b/VirtualRobot/examples/Jacobi/JacobiWindow.h
@@ -31,7 +31,7 @@ class JacobiWindow : public QMainWindow
Q_OBJECT
public:
JacobiWindow(std::string& sRobotFilename);
- ~JacobiWindow();
+ ~JacobiWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -46,7 +46,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void collisionModel();
diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.h b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.h
index 8970deed39fe48ec41844716d22e56570b2b1016..1067e59d2ebd192c676c1e64f80d59340a40de39 100644
--- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.h
+++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.h
@@ -33,7 +33,7 @@ class ReachabilityMapWindow : public QMainWindow
Q_OBJECT
public:
ReachabilityMapWindow(std::string& sRobotFile, std::string& reachFile, std::string& objFile, std::string& eef);
- ~ReachabilityMapWindow();
+ ~ReachabilityMapWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -43,7 +43,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void updateVisu();
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.h b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
index 4c677c0dbf34e9cd2d384f8e5821966848b9600a..b3bbf5016629fb543fd06fcf9c20a94eab837c09 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.h
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
@@ -34,7 +34,7 @@ class showRobotWindow : public QMainWindow
Q_OBJECT
public:
showRobotWindow(std::string& sRobotFilename);
- ~showRobotWindow();
+ ~showRobotWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -44,7 +44,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void rebuildVisualization();
diff --git a/VirtualRobot/examples/SceneViewer/showSceneWindow.h b/VirtualRobot/examples/SceneViewer/showSceneWindow.h
index 6a0bb3f3d33706eb4ec221fe297ecdaf70b967c1..e3c4f6190ad111a256e60125f24767e67fca4eb0 100644
--- a/VirtualRobot/examples/SceneViewer/showSceneWindow.h
+++ b/VirtualRobot/examples/SceneViewer/showSceneWindow.h
@@ -31,7 +31,7 @@ class showSceneWindow : public QMainWindow
Q_OBJECT
public:
showSceneWindow(std::string& sSceneFile);
- ~showSceneWindow();
+ ~showSceneWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -41,7 +41,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void loadScene();
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.h b/VirtualRobot/examples/reachability/reachabilityWindow.h
index 5484e7f72567c63dc16aad84a12712a358e9b023..36a0d41cce9c3d5a8718892d1c4b3f634e599260 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.h
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.h
@@ -32,7 +32,7 @@ class reachabilityWindow : public QMainWindow
Q_OBJECT
public:
reachabilityWindow(std::string& sRobotFile, std::string& reachFile, Eigen::Vector3f& axisTCP);
- ~reachabilityWindow();
+ ~reachabilityWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -42,7 +42,7 @@ public slots:
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void selectRobot();
void createReach();
diff --git a/VirtualRobot/examples/stability/stabilityWindow.h b/VirtualRobot/examples/stability/stabilityWindow.h
index 144973100ae506092ccc15eb5663119fc04112ed..84d8a7ae12b1d7dcef0c31ead46acb7a6ddff82c 100644
--- a/VirtualRobot/examples/stability/stabilityWindow.h
+++ b/VirtualRobot/examples/stability/stabilityWindow.h
@@ -29,7 +29,7 @@ class stabilityWindow : public QMainWindow
Q_OBJECT
public:
stabilityWindow(std::string& sRobotFile);
- ~stabilityWindow();
+ ~stabilityWindow() override;
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
@@ -38,7 +38,7 @@ public slots:
/*! Closes the window and exits SoQt runloop. */
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
- void closeEvent(QCloseEvent* event);
+ void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
void selectRobot();
void collisionModel();
diff --git a/VirtualRobot/math/AbstractFunctionR1R2.cpp b/VirtualRobot/math/AbstractFunctionR1R2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fdebaa8f858a7511fa074f94caeca9f370871a8
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR1R2.cpp
@@ -0,0 +1,29 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "AbstractFunctionR1R2.h"
+#include <Eigen/Geometry>
+
+using namespace math;
+
+Eigen::Vector2f math::AbstractFunctionR1R2::GetNormalizedNormal(float t){
+ Eigen::Rotation2Df rot(M_PI_2);
+ return (rot * GetDerivative(t)).normalized();
+}
diff --git a/VirtualRobot/math/AbstractFunctionR1R2.h b/VirtualRobot/math/AbstractFunctionR1R2.h
new file mode 100644
index 0000000000000000000000000000000000000000..dbd1ced56eecb8e320657d11b747b8f6cb81c129
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR1R2.h
@@ -0,0 +1,44 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_AbstractFunctionR1R2
+#define math_HapticExplorationLibrary_AbstractFunctionR1R2
+
+#include "MathForwardDefinitions.h"
+
+
+
+
+namespace math
+{
+
+class AbstractFunctionR1R2
+{
+public:
+ AbstractFunctionR1R2();
+ virtual Eigen::Vector2f Get(float t) = 0;
+ virtual Eigen::Vector2f GetDerivative(float t)= 0;
+ Eigen::Vector2f GetNormalizedNormal(float t);
+
+private:
+};
+}
+
+#endif // math_HapticExplorationLibrary_AbstractFunctionR1R2
diff --git a/VirtualRobot/math/AbstractFunctionR1R3.cpp b/VirtualRobot/math/AbstractFunctionR1R3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..75d42bdb145d205b78cf0896b81c45c6a08a9a24
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR1R3.cpp
@@ -0,0 +1,95 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "AbstractFunctionR1R3.h"
+
+using namespace math;
+
+AbstractFunctionR1R3::AbstractFunctionR1R3()
+{
+}
+
+
+float AbstractFunctionR1R3::FindClosestPoint(Eigen::Vector3f p, float t1, float t2, int segments)
+{
+ float minT = t1;
+ float minD = (Get(minT)-p).squaredNorm();
+ for (int i = 1; i <= segments; i++)
+ {
+ float t = t1 + (t2 - t1) * i / segments;
+ float d = (Get(t)-p).squaredNorm();
+ if (d < minD)
+ {
+ minT = t;
+ minD = d;
+ }
+ }
+ return minT;
+
+}
+
+
+float AbstractFunctionR1R3::MoveLengthOnCurve(float x, float l, int steps)
+{
+ float dl = l / steps;
+ for (int i = 0; i < steps; i++)
+ {
+ float d = GetDerivative(x).norm();
+ x += dl / (float)std::sqrt(1 + d * d); // move by dl on curve: tangential vector: (1, df/dx), normalize to dl
+ }
+ return x;
+}
+
+
+float AbstractFunctionR1R3::GetLength(float t1, float t2, int steps)
+{
+ Eigen::Vector3f p0 = Get(t1);
+ float l = 0;
+ for (int i = 1; i <= steps; i++)
+ {
+ float t = t1 + (t2 - t1) / steps * i;
+ Eigen::Vector3f p1 = Get(t);
+ l += (p1 - p0).norm();
+ p0 = p1;
+ }
+ return l;
+}
+
+
+std::vector<Eigen::Vector3f> AbstractFunctionR1R3::Sample(float t1, float t2, int segments)
+{
+ std::vector<Eigen::Vector3f> result;
+ for(float f : Segments(t1, t2, segments)){
+ result.push_back(Get(f));
+ }
+ return result;
+}
+
+
+std::vector<float> AbstractFunctionR1R3::Segments(float t1, float t2, int segments)
+{
+ std::vector<float> result;
+
+ for (int i = 0; i <= segments; i++)
+ {
+ result.push_back(t1 + (t2 - t1) * i / segments);
+ }
+ return result;
+}
diff --git a/VirtualRobot/math/AbstractFunctionR1R3.h b/VirtualRobot/math/AbstractFunctionR1R3.h
new file mode 100644
index 0000000000000000000000000000000000000000..83765b5a8c042827b33ffff2b0912fa3345ef6c1
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR1R3.h
@@ -0,0 +1,45 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_AbstractFunctionR1R3
+#define math_HapticExplorationLibrary_AbstractFunctionR1R3
+
+#include "SimpleAbstractFunctionR1R3.h"
+
+
+
+namespace math
+{
+ class AbstractFunctionR1R3 :
+ public SimpleAbstractFunctionR1R3
+ {
+ public:
+ AbstractFunctionR1R3();
+ virtual Eigen::Vector3f GetDerivative(float t)= 0;
+ float FindClosestPoint(Eigen::Vector3f p, float t1, float t2, int segments);
+ float MoveLengthOnCurve(float x, float l, int steps);
+ float GetLength(float t1, float t2, int steps);
+ std::vector<Eigen::Vector3f> Sample(float t1, float t2, int segments);
+ std::vector<float> Segments(float t1, float t2, int segments);
+ private:
+ };
+}
+
+#endif // math_HapticExplorationLibrary_AbstractFunctionR1R3
diff --git a/VirtualRobot/math/AbstractFunctionR2R3.cpp b/VirtualRobot/math/AbstractFunctionR2R3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..565f846f7abe0a801e1a50e9cac8f0d0d0c3d6c9
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR2R3.cpp
@@ -0,0 +1,62 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "AbstractFunctionR2R3.h"
+#include "Contact.h"
+#include "Plane.h"
+
+
+using namespace math;
+
+AbstractFunctionR2R3::AbstractFunctionR2R3()
+{
+}
+
+Eigen::Vector3f AbstractFunctionR2R3::GetNormal(float u, float v)
+{
+ Eigen::Vector3f dduVec = GetDdu(u, v);
+ Eigen::Vector3f ddvVec = GetDdv(u, v);
+ return dduVec.cross(ddvVec);
+}
+
+Plane AbstractFunctionR2R3::GetContactPlane(float u, float v)
+{
+ return Plane(GetPoint(u, v), GetDdu(u, v), GetDdv(u, v));
+}
+
+Contact AbstractFunctionR2R3::GetContact(float u, float v)
+{
+ return Contact(GetPoint(u, v), GetNormal(u, v));
+}
+
+Eigen::Vector3f AbstractFunctionR2R3::GetNormal(Eigen::Vector2f uv) { return GetNormal(uv.x(), uv.y()); }
+
+Eigen::Vector3f AbstractFunctionR2R3::GetDdu(Eigen::Vector2f uv) { return GetDdu(uv.x(), uv.y()); }
+
+Eigen::Vector3f AbstractFunctionR2R3::GetDdv(Eigen::Vector2f uv) { return GetDdv(uv.x(), uv.y()); }
+
+Eigen::Vector2f AbstractFunctionR2R3::GetUVFromPos(Eigen::Vector3f pos)
+{
+ float u, v;
+ GetUV(pos, u, v);
+ return Eigen::Vector2f(u, v);
+}
+
+Eigen::Vector3f AbstractFunctionR2R3::GetVector(Eigen::Vector2f pos, Eigen::Vector2f dir) { return GetDdu(pos) * dir.x() + GetDdv(pos) * dir.y(); }
diff --git a/VirtualRobot/math/AbstractFunctionR2R3.h b/VirtualRobot/math/AbstractFunctionR2R3.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea57b16c6f255faa1e8ab34ed5dc3ed5c8bc7004
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR2R3.h
@@ -0,0 +1,73 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_AbstractFunctionR2R3
+#define math_HapticExplorationLibrary_AbstractFunctionR2R3
+
+#include "SimpleAbstractFunctionR2R3.h"
+
+
+
+namespace math
+{
+
+
+class AbstractFunctionR2R3:
+ public SimpleAbstractFunctionR2R3
+{
+public:
+ enum ProjectionType { SimpleProjection, FindClosestPointType };
+
+ AbstractFunctionR2R3();
+
+ virtual Eigen::Vector3f GetDdu(float u, float v)= 0;
+ virtual Eigen::Vector3f GetDdv(float u, float v)= 0;
+
+ Eigen::Vector3f GetNormal(float u, float v);
+
+ Plane GetContactPlane(float u, float v);
+
+ Contact GetContact(float u, float v);
+ virtual void GetUV(Eigen::Vector3f pos, float &u, float &v)= 0;
+
+ Eigen::Vector3f GetNormal(Eigen::Vector2f uv);
+ Eigen::Vector3f GetDdu(Eigen::Vector2f uv);
+ Eigen::Vector3f GetDdv(Eigen::Vector2f uv);
+ Plane GetContactPlane(Eigen::Vector2f uv) ;
+ Contact GetContact(Eigen::Vector2f uv) ;
+ Eigen::Vector2f GetUVFromPos(Eigen::Vector3f pos) ;
+ Eigen::Vector3f GetVector(Eigen::Vector2f pos, Eigen::Vector2f dir);
+
+ float GetSquareDistance(Eigen::Vector3f pos, ProjectionType projection);
+
+ float GetDistance(Eigen::Vector3f pos, ProjectionType projection);
+
+ Eigen::Vector3f GetPointOnFunction(Eigen::Vector3f pos, ProjectionType projection);
+ Eigen::Vector3f ProjectPointOntoFunction(Eigen::Vector3f pos);
+ Eigen::Vector3f FindClosestPoint(Eigen::Vector3f pos, float epsilon = 0.001f);
+ LineR2 ProjectVectorToUV(Eigen::Vector3f pos, Eigen::Vector3f dir);
+
+
+private:
+ // void Step(Eigen::Vector3f pos, float u0, float v0, float& u1, float& v1);
+};
+}
+
+#endif // math_HapticExplorationLibrary_AbstractFunctionR2R3
diff --git a/VirtualRobot/math/AbstractFunctionR3R1.h b/VirtualRobot/math/AbstractFunctionR3R1.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9049a1e5e7cd85d9aea6f35c9c57c5aafa1f3e7
--- /dev/null
+++ b/VirtualRobot/math/AbstractFunctionR3R1.h
@@ -0,0 +1,42 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_AbstractFunctionR3R1
+#define math_HapticExplorationLibrary_AbstractFunctionR3R1
+
+
+#include "SimpleAbstractFunctionR3R1.h"
+
+
+namespace math
+{
+ class AbstractFunctionR3R1 :
+ public SimpleAbstractFunctionR3R1
+ {
+ public:
+ virtual float GetDdx(Eigen::Vector3f pos) = 0;
+ virtual float GetDdy(Eigen::Vector3f pos) = 0;
+ virtual float GetDdz(Eigen::Vector3f pos) = 0;
+
+ private:
+ };
+}
+
+#endif // math_HapticExplorationLibrary_AbstractFunctionR3R1
diff --git a/VirtualRobot/math/Array3D.h b/VirtualRobot/math/Array3D.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea57d924f95e3761c38c1769202437b80648a2e4
--- /dev/null
+++ b/VirtualRobot/math/Array3D.h
@@ -0,0 +1,77 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Array3D
+#define math_Array3D
+
+#include "MathForwardDefinitions.h"
+
+#include "Index3.h"
+
+
+namespace math
+{
+ template<class T>
+ class Array3D
+ {
+ public:
+ Array3D(int size) :
+ size(size)
+ {
+ data = boost::shared_ptr<std::vector<T>>(new std::vector<T>);
+ data->resize(size*size*size);
+ }
+
+ T Get(Index3 index)
+ {
+ return data->at(index.X() +
+ size * index.Y() +
+ size * size * index.Z());
+ }
+
+ T Get(int i, int j, int k)
+ {
+ return data->at(i +
+ size * j +
+ size * size * k );
+
+ }
+
+ void Set(Index3 index, T value)
+ {
+ data->at( index.X() +
+ size * index.Y() +
+ size * size * index.Z()) = value;
+ }
+ void Set(int i, int j, int k, T value)
+ {
+ data->at( i +
+ size * j +
+ size * size * k )= value;
+ }
+
+ private:
+ boost::shared_ptr<std::vector<T>> data;
+ //std::vector<T> data;
+ int size;
+ };
+}
+
+#endif //math_Array3D
diff --git a/VirtualRobot/math/Bezier.cpp b/VirtualRobot/math/Bezier.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f1ab39bacc7414224d11fe466e1b4edb46059cf
--- /dev/null
+++ b/VirtualRobot/math/Bezier.cpp
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Bezier.h"
+
+using namespace math;
+math::Bezier::Bezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
+ : p0(p0), p1(p1), p2(p2), p3(p3)
+{
+}
+
+Eigen::Vector3f math::Bezier::Get(float t)
+{
+ return CubicBezierPoint(p0, p1, p2, p3, t);
+}
+
+Eigen::Vector3f math::Bezier::GetDerivative(float t)
+{
+ return CubicBezierDerivative(p0, p1, p2, p3, t);
+}
+
+float math::Bezier::Pow3(float x)
+{
+ return x * x * x;
+}
+
+float math::Bezier::Pow2(float x)
+{
+ return x * x;
+}
+
+Eigen::Vector3f math::Bezier::CubicBezierPoint(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, float t)
+{
+ return Pow3(1 - t) * p0 + 3 * t * Pow2(1 - t) * p1 + 3 * Pow2(t) * (1 - t) * p2 + Pow3(t) * p3;
+}
+
+std::vector<Eigen::Vector3f> math::Bezier::CubicBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, int steps)
+{
+ std::vector<Eigen::Vector3f> points;
+ for (int i = 0; i <= steps; i++)
+ {
+ points.push_back(CubicBezierPoint(p0, p1, p2, p3, 1.f / steps * i));
+ }
+ return points;
+}
+
+Eigen::Vector3f math::Bezier::CubicBezierDerivative(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, float t)
+{
+ return -3 * Pow2(1 - t) * p0 + 3 * Pow2(1 - t) * p1 - 6 * (1 - t) * t * p1 + 6 * (1 - t) * t * p2 - 3 * Pow2(t) * p2 + 3 * Pow2(t) * p3;
+}
diff --git a/VirtualRobot/math/Bezier.h b/VirtualRobot/math/Bezier.h
new file mode 100644
index 0000000000000000000000000000000000000000..1cd2a84376e974a82ea74659418d201b058080c0
--- /dev/null
+++ b/VirtualRobot/math/Bezier.h
@@ -0,0 +1,59 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Bezier
+#define math_Bezier
+
+#include "MathForwardDefinitions.h"
+
+#include "AbstractFunctionR1R3.h"
+
+namespace math
+{
+
+ class Bezier :
+ public AbstractFunctionR1R3
+ {
+ public:
+ Bezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3);
+
+ Eigen::Vector3f P0(){return p0;}
+ Eigen::Vector3f P1(){return p1;}
+ Eigen::Vector3f P2(){return p2;}
+ Eigen::Vector3f P3(){return p3;}
+
+ Eigen::Vector3f Get(float t) override;
+ Eigen::Vector3f GetDerivative(float t);
+ static Eigen::Vector3f CubicBezierPoint(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, float t);
+ static std::vector<Eigen::Vector3f> CubicBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, int steps);
+ static Eigen::Vector3f CubicBezierDerivative(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, float t);
+
+ private:
+ Eigen::Vector3f p0;
+ Eigen::Vector3f p1;
+ Eigen::Vector3f p2;
+ Eigen::Vector3f p3;
+
+ static float Pow3(float x);
+ static float Pow2(float x);
+ };
+}
+
+#endif //math_Bezier
diff --git a/VirtualRobot/math/Contact.cpp b/VirtualRobot/math/Contact.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce600500b92c38382aa6dbfee541fe234ced7fe0
--- /dev/null
+++ b/VirtualRobot/math/Contact.cpp
@@ -0,0 +1,58 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Contact.h"
+#include "Helpers.h"
+using namespace math;
+
+
+Contact::Contact()
+ : position(Eigen::Vector3f(0,0,0)), normal(Eigen::Vector3f(0,0,0))
+{
+}
+
+Contact::Contact(Eigen::Vector3f position, Eigen::Vector3f normal)
+ : position(position), normal(normal)
+{
+}
+
+Contact::Contact(float px, float py, float pz, float nx, float ny, float nz)
+ : position(px, py, pz), normal(nx, ny, nz)
+{
+
+}
+
+Contact Contact::Normalized()
+{
+ return Contact(position, normal.normalized());
+}
+
+std::string Contact::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << position.transpose() << ") (" << normal.transpose() << ")";
+ return ss.str();
+}
+
+Contact Contact::Lerp(Contact a, Contact b, float f)
+{
+ return Contact(Helpers::Lerp(a.position, b.position, f),
+ Helpers::Lerp(a.normal, b.normal, f).normalized());
+}
diff --git a/VirtualRobot/math/Contact.h b/VirtualRobot/math/Contact.h
new file mode 100644
index 0000000000000000000000000000000000000000..aebb33efd2f6e3b3c36a96289fa184d726f61541
--- /dev/null
+++ b/VirtualRobot/math/Contact.h
@@ -0,0 +1,51 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_Contact
+#define math_HapticExplorationLibrary_Contact
+
+#include "MathForwardDefinitions.h"
+#include <sstream>
+
+
+
+namespace math
+{
+ class Contact
+ {
+ public:
+ Contact();
+ Contact(Eigen::Vector3f position, Eigen::Vector3f normal);
+ Contact(float px, float py, float pz, float nx, float ny, float nz);
+
+ Eigen::Vector3f Position() { return position; }
+ Eigen::Vector3f Normal() { return normal; }
+
+ Contact Normalized();
+
+ std::string ToString();
+ static Contact Lerp(Contact a, Contact b, float f);
+ private:
+ Eigen::Vector3f position;
+ Eigen::Vector3f normal;
+ };
+}
+
+#endif // math_HapticExplorationLibrary_Contact
diff --git a/VirtualRobot/math/ContactList.cpp b/VirtualRobot/math/ContactList.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9374cd12ee6b67bde261a2c4d0d9d04bf5dd2ff7
--- /dev/null
+++ b/VirtualRobot/math/ContactList.cpp
@@ -0,0 +1,70 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "ContactList.h"
+#include <sstream>
+
+
+using namespace math;
+
+ContactList::ContactList()
+ : std::vector<Contact>()
+{
+}
+
+std::vector<Eigen::Vector3f> ContactList::GetPoints()
+{
+ std::vector<Eigen::Vector3f> points;
+ for(Contact c: *this){
+ points.push_back(c.Position());
+ }
+ return points;
+}
+
+Contact ContactList::Last()
+{
+ if (size() == 0){
+ throw std::runtime_error("size == 0");
+ }else{
+ return this->at(size()-1);
+ }
+}
+
+std::string ContactList::ToString()
+{
+ std::stringstream ss;
+ bool first1 = true;
+ for(Contact c : *this)
+ {
+ if (first1)
+ {
+ ss << "[" << c.ToString() << "]";
+ first1 = false;
+ }
+ else
+ {
+ ss << ", [" << c.ToString() << "]";
+ }
+ }
+ return ss.str();
+
+}
+
+
diff --git a/VirtualRobot/math/ContactList.h b/VirtualRobot/math/ContactList.h
new file mode 100644
index 0000000000000000000000000000000000000000..3318c2c539f9779cf87ad9708638e36fec945b73
--- /dev/null
+++ b/VirtualRobot/math/ContactList.h
@@ -0,0 +1,42 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_ContactList
+#define math_HapticExplorationLibrary_ContactList
+
+#include "Contact.h"
+
+namespace math
+{
+
+ class ContactList :
+ public std::vector<Contact>
+ {
+ public:
+ ContactList();
+ std::vector<Eigen::Vector3f> GetPoints();
+ Contact Last();
+ std::string ToString();
+
+ private:
+ };
+}
+
+#endif // math_HapticExplorationLibrary_ContactList
diff --git a/VirtualRobot/math/DataR3R1.cpp b/VirtualRobot/math/DataR3R1.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6e48c43ad62e90f6504eeacee40da1c1883cf1b
--- /dev/null
+++ b/VirtualRobot/math/DataR3R1.cpp
@@ -0,0 +1,36 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "DataR3R1.h"
+
+using namespace math;
+
+
+DataR3R1::DataR3R1(Eigen::Vector3f position, float value)
+ :position(position), value(value)
+{
+}
+
+std::string DataR3R1::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << position << ") " << value;
+ return ss.str();
+}
diff --git a/VirtualRobot/math/DataR3R1.h b/VirtualRobot/math/DataR3R1.h
new file mode 100644
index 0000000000000000000000000000000000000000..48632cff101fd84e5530f3db856b026ee53c87c0
--- /dev/null
+++ b/VirtualRobot/math/DataR3R1.h
@@ -0,0 +1,42 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_DataR3R1
+#define math_DataR3R1
+
+#include "MathForwardDefinitions.h"
+
+namespace math
+{
+
+ class DataR3R1
+ {
+ public:
+ Eigen::Vector3f Position(){return position;}
+ float Value(){return value;}
+ DataR3R1(Eigen::Vector3f position, float value);
+ std::string ToString();
+ private:
+ Eigen::Vector3f position;
+ float value;
+ };
+}
+
+#endif // math_DataR3R1
diff --git a/VirtualRobot/math/GaussianImplicitSurface3D.cpp b/VirtualRobot/math/GaussianImplicitSurface3D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac3e17d766c5b519a57b5146f763b33d3675ad5c
--- /dev/null
+++ b/VirtualRobot/math/GaussianImplicitSurface3D.cpp
@@ -0,0 +1,116 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "GaussianImplicitSurface3D.h"
+
+using namespace math;
+
+
+
+
+void GaussianImplicitSurface3D::Calculate(std::vector<DataR3R1> samples, float noise)
+{
+ std::vector<DataR3R1> shiftedSamples;
+ std::vector<Eigen::Vector3f> points;
+ Eigen::VectorXf values(samples.size());
+ int i = 0;
+ mean = Average(samples);
+ for(DataR3R1 d : samples){
+ Eigen::Vector3f shiftedPos = d.Position()- mean;
+ shiftedSamples.push_back(DataR3R1(shiftedPos, d.Value()));
+ points.push_back(shiftedPos);
+ values(i++) = d.Value();
+ }
+ R = 0;
+ for(Eigen::Vector3f p1 : points){
+ for(Eigen::Vector3f p2 : points){
+ if((p1-p2).norm() > R) R= (p1-p2).norm();
+ }
+ }
+ R = std::sqrt(R);
+
+ covariance = CalculateCovariance(points, R, noise);
+ alpha = MatrixSolve(covariance, values);
+ //L = Cholesky(covariance);
+ //alpha = FitModel(L, samples);
+ this->samples = shiftedSamples;
+
+}
+
+float GaussianImplicitSurface3D::Get(Eigen::Vector3f pos)
+{
+ return Predict(pos);
+}
+
+float GaussianImplicitSurface3D::Predict(Eigen::Vector3f pos)
+{
+ Eigen::VectorXf Cux(samples.size());
+ int i = 0;
+ pos = pos - mean;
+ for (DataR3R1 d : samples){
+ Cux(i++) = Kernel(pos, d.Position(), R);
+ }
+ return Cux.dot( alpha);
+
+}
+
+Eigen::MatrixXf GaussianImplicitSurface3D::CalculateCovariance(std::vector<Eigen::Vector3f> points, float R, float noise)
+{
+ Eigen::MatrixXf covariance = Eigen::MatrixXf(points.size(), points.size());
+
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ for (size_t j = i; j < points.size(); j++)
+ {
+ float cov = Kernel(points.at(i), points.at(j), R);
+ covariance(i, j) = cov;
+ covariance(j, i) = cov;
+ }
+ }
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ covariance(i, i) += noise * noise;
+ }
+ return covariance;
+}
+
+
+
+Eigen::VectorXf GaussianImplicitSurface3D::MatrixSolve(Eigen::MatrixXf a, Eigen::VectorXf b)
+{
+ return a.colPivHouseholderQr().solve(b);
+}
+
+
+float GaussianImplicitSurface3D::Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R)
+{
+ float r = (p1-p2).norm();
+ return 2 * r*r*r + 3 * R * r*r + R*R*R;
+}
+
+Eigen::Vector3f GaussianImplicitSurface3D::Average(std::vector<DataR3R1> samples)
+{
+ Eigen::Vector3f sum = Eigen::Vector3f(0,0,0);
+ if (samples.size() == 0) return sum;
+ for(DataR3R1 d : samples){
+ sum += d.Position();
+ }
+ return sum / samples.size();
+}
diff --git a/VirtualRobot/math/GaussianImplicitSurface3D.h b/VirtualRobot/math/GaussianImplicitSurface3D.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e4bb3f9c4736d6029cbdb8086fe2e5d85242a4c
--- /dev/null
+++ b/VirtualRobot/math/GaussianImplicitSurface3D.h
@@ -0,0 +1,59 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_GaussianImplicitSurface3D
+#define math_GaussianImplicitSurface3D
+
+#include "MathForwardDefinitions.h"
+#include "DataR3R1.h"
+#include "SimpleAbstractFunctionR3R1.h"
+
+namespace math
+{
+
+class GaussianImplicitSurface3D :
+ public SimpleAbstractFunctionR3R1
+{
+public:
+ //GaussianImplicitSurface3D();
+ void Calculate(std::vector<DataR3R1> samples, float noise);
+ float Get(Eigen::Vector3f pos) override;
+
+private:
+ Eigen::MatrixXf covariance;
+ Eigen::VectorXf alpha;
+ std::vector<DataR3R1> samples;
+ float R;
+ Eigen::Vector3f mean;
+
+
+ float Predict(Eigen::Vector3f pos);
+ static Eigen::MatrixXf CalculateCovariance(std::vector<Eigen::Vector3f> points, float R, float noise);
+ //static VectorXf Cholesky(MatrixXf matrix);
+ //static VectorXf FitModel(MatrixXf L, List<DataR3R1> targets);
+ //VectorXf SpdMatrixSolve(MatrixXf a, bool IsUpper, VectorXf b);
+ static Eigen::VectorXf MatrixSolve(Eigen::MatrixXf a, Eigen::VectorXf b);
+ static float Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R);
+ //static double[,] Transpose(double[,] a)
+ static Eigen::Vector3f Average(std::vector<DataR3R1> samples);
+};
+}
+
+#endif // math_GaussianImplicitSurface3D
diff --git a/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp b/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d41d1d362150969bccb4b77f320f388d63cfcf6c
--- /dev/null
+++ b/VirtualRobot/math/GaussianImplicitSurface3DNormals.cpp
@@ -0,0 +1,191 @@
+#include "GaussianImplicitSurface3DNormals.h"
+
+using namespace math;
+
+GaussianImplicitSurface3DNormals::GaussianImplicitSurface3DNormals()
+{
+
+}
+
+void GaussianImplicitSurface3DNormals::Calculate(ContactList samples, float noise, float normalNoise, float normalScale)
+{
+ ContactList shiftedSamples;
+ std::vector<Eigen::Vector3f> points;
+ std::vector<Eigen::Vector3f> pointsOriginal;
+ mean = Average(samples);
+ for(Contact d : samples){
+ Eigen::Vector3f shiftedPos = d.Position()- mean;
+ shiftedSamples.push_back(Contact(shiftedPos, d.Normal()*normalScale));
+ points.push_back(shiftedPos);
+ pointsOriginal.push_back(d.Position());
+ }
+ R = 0;
+ for(Eigen::Vector3f p1 : points){
+ for(Eigen::Vector3f p2 : points){
+ if((p1-p2).norm() > R) R= (p1-p2).norm();
+ }
+ }
+ R = std::sqrt(R);
+
+ covariance = CalculateCovariance(pointsOriginal, R, noise, normalNoise);
+ Eigen::VectorXf y(samples.size()*4);
+ for (uint i = 0; i < samples.size(); i++)
+ {
+ y(i*4) = 0;
+ y(i * 4 + 1) = samples.at(i).Normal().x();
+ y(i * 4 + 2) = samples.at(i).Normal().y();
+ y(i * 4 + 3) = samples.at(i).Normal().z();
+ }
+ alpha = MatrixSolve(covariance, y);
+ this->samples = shiftedSamples;
+}
+
+
+float GaussianImplicitSurface3DNormals::Get(Eigen::Vector3f pos)
+{
+ return Predict(pos);
+}
+
+Eigen::VectorXf GaussianImplicitSurface3DNormals::getCux(Eigen::Vector3f pos)
+{
+ Eigen::VectorXf Cux (samples.size() * 4);
+ for (std::size_t i = 0; i < samples.size(); i++)
+ {
+ Cux(i * 4) = Kernel(pos, samples.at(i).Position(), R);
+ Cux(i * 4 + 1) = Kernel_dj(pos, samples.at(i).Position(), R, 0);
+ Cux(i * 4 + 2) = Kernel_dj(pos, samples.at(i).Position(), R, 1);
+ Cux(i * 4 + 3) = Kernel_dj(pos, samples.at(i).Position(), R, 2);
+ }
+ return Cux;//VectorXf;
+}
+float GaussianImplicitSurface3DNormals::Predict(Eigen::Vector3f pos)
+{
+ Eigen::VectorXf Cux(4*samples.size());
+ pos = pos - mean;
+ Cux=getCux(pos);
+ return Cux.dot(alpha);
+
+}
+Eigen::MatrixXf GaussianImplicitSurface3DNormals::CalculateCovariance(std::vector<Eigen::Vector3f> points, float R, float noise, float normalNoise)
+{
+ Eigen::MatrixXf covariance = Eigen::MatrixXf(points.size()*4, points.size()*4);
+
+ for (size_t i = 0; i < points.size()*4; i++)
+ {
+ for (size_t j = i; j < points.size()*4; j++)
+ {
+ float cov = Kernel(points.at(i/4), points.at(j/4), R, i%4 , j%4);
+ covariance(i, j) = cov;
+ covariance(j, i) = cov;
+ }
+ }
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ if(i%4==0){
+ covariance(i, i) += i % 4 == 0 ? noise*noise : normalNoise;
+ }
+
+ }
+ return covariance;
+}
+Eigen::VectorXf GaussianImplicitSurface3DNormals::MatrixSolve(Eigen::MatrixXf a, Eigen::VectorXf b)
+{
+ return a.colPivHouseholderQr().solve(b);
+}
+
+Eigen::Vector3f GaussianImplicitSurface3DNormals::Average(ContactList samples)
+{
+ Eigen::Vector3f sum = Eigen::Vector3f(0,0,0);
+ if (samples.size() == 0) return sum;
+ for(Contact d : samples){
+ sum += d.Position();
+ }
+ return sum / samples.size();
+}
+
+float GaussianImplicitSurface3DNormals::Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R)
+{
+ float r = (p1-p2).norm();
+ return 2 * r*r*r + 3 * R * r*r + R*R*R;
+}
+
+float GaussianImplicitSurface3DNormals::Kernel_dx(float x, float /*y*/, float /*z*/, float r, float R)
+{
+ return 6 * x * (r + R);
+}
+
+float GaussianImplicitSurface3DNormals::Kernel_ddx(float x, float /*y*/, float /*z*/, float r, float R)
+{
+ if (r == 0) return 6 * R;
+ return 6 * (R + r) + 6 * x * x / r;
+}
+
+float GaussianImplicitSurface3DNormals::Kernel_dxdy(float x, float y, float /*z*/, float r, float /*R*/)
+{
+ if (r == 0) return 0;
+ return 6 * x * y / r;
+}
+
+
+float GaussianImplicitSurface3DNormals::Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i, int j)
+{
+ if (i == 0 && j == 0) return Kernel(p1, p2, R);
+ if (i == 0) return Kernel_dj(p1, p2, R, j - 1);
+ if (j == 0) return Kernel_di(p1, p2, R, i - 1);
+ return Kernel_didj(p1, p2, R, i - 1, j - 1);
+}
+
+float GaussianImplicitSurface3DNormals::Kernel_di(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i)
+{
+ float r = (p1-p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ swap(x, y, z, i);
+ return Kernel_dx(x, y, z, r, R);
+}
+float GaussianImplicitSurface3DNormals::Kernel_dj(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int j)
+{
+ float r = (p1-p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ swap(x, y, z, j);
+ return -Kernel_dx(x, y, z, r, R);
+}
+
+float GaussianImplicitSurface3DNormals::Kernel_didj(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i, int j)
+{
+ float r = (p1-p2).norm();
+ float x = p1.x() - p2.x();
+ float y = p1.y() - p2.y();
+ float z = p1.z() - p2.z();
+ if (i == j)
+ {
+ swap(x, y, z, i);
+ return -Kernel_ddx(x, y, z, r, R);
+ }
+ std::vector<float> a(3);
+ a.at(0)=x;
+ a.at(1)=y;
+ a.at(2)=z;
+
+ x = a.at(i);
+ y = a.at(j);
+ z = a.at(3 - i - j);
+ return -Kernel_dxdy(x, y, z, r, R);
+
+}
+
+
+void GaussianImplicitSurface3DNormals::swap(float &x, float &y, float &z, int index) //TODO ref
+{
+ switch (index)
+ {
+ case 0: break;
+ case 1: math::Helpers::Swap(x, y); break;
+ case 2: math::Helpers::Swap(x, z); break;
+ default: throw std::invalid_argument("index must be >= 0 and <= 2" );
+ }
+}
+
diff --git a/VirtualRobot/math/GaussianImplicitSurface3DNormals.h b/VirtualRobot/math/GaussianImplicitSurface3DNormals.h
new file mode 100644
index 0000000000000000000000000000000000000000..789686e80ca581376904f2e6ba2995625dc4d1b2
--- /dev/null
+++ b/VirtualRobot/math/GaussianImplicitSurface3DNormals.h
@@ -0,0 +1,71 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Andreea Tulbure (andreea dot tulbure at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_GaussianImplicitSurfaceNormals3D
+#define math_GaussianImplicitSurfaceNormals3D
+
+#include "MathForwardDefinitions.h"
+#include "Contact.h"
+#include "Helpers.h"
+#include "SimpleAbstractFunctionR3R1.h"
+#include "ContactList.h"
+
+namespace math
+{
+
+class GaussianImplicitSurface3DNormals :
+ public SimpleAbstractFunctionR3R1
+{
+public:
+ GaussianImplicitSurface3DNormals();
+ void Calculate(ContactList samples, float noise, float normalNoise, float normalScale);
+ float Get(Eigen::Vector3f pos) override;
+
+private:
+ Eigen::MatrixXf covariance;
+ Eigen::VectorXf alpha;
+ ContactList samples;
+ float R;
+ float R3;
+ Eigen::Vector3f mean;
+
+ Eigen::MatrixXf Cinv;
+
+ float Predict(Eigen::Vector3f pos);
+ static Eigen::MatrixXf CalculateCovariance(std::vector<Eigen::Vector3f> points, float R, float noise, float normalNoise);
+ //static VectorXf Cholesky(MatrixXf matrix);
+ //static VectorXf FitModel(MatrixXf L, List<DataR3R1> targets);
+ //VectorXf SpdMatrixSolve(MatrixXf a, bool IsUpper, VectorXf b);
+ static Eigen::VectorXf MatrixSolve(Eigen::MatrixXf a, Eigen::VectorXf b);
+ static float Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R);
+ static float Kernel(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i, int j);
+ static float Kernel_dx(float x, float y, float z, float r, float R);
+ static float Kernel_dxdy(float x, float y, float z, float r, float R);
+ static float Kernel_ddx(float x, float y, float z, float r, float R);
+ static float Kernel_di(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i);
+ static float Kernel_dj(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i);
+ static float Kernel_didj(Eigen::Vector3f p1, Eigen::Vector3f p2, float R, int i, int j);
+ static void swap(float &x, float &y,float &z, int index);
+ static Eigen::Vector3f Average(ContactList samples);
+ Eigen::VectorXf getCux(Eigen::Vector3f pos);
+};
+}
+
+#endif // math_GaussianImplicitSurface3DNormals
diff --git a/VirtualRobot/math/GaussianObjectModel.cpp b/VirtualRobot/math/GaussianObjectModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11afff30beab09f687e5c3d78271392fd57ef262
--- /dev/null
+++ b/VirtualRobot/math/GaussianObjectModel.cpp
@@ -0,0 +1,62 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "GaussianObjectModel.h"
+#include "MathForwardDefinitions.h"
+
+using namespace math;
+
+GaussianObjectModel::GaussianObjectModel(float noise)
+ : noise(noise)
+{
+ model = gpModel = GaussianImplicitSurface3DPtr(new GaussianImplicitSurface3D());
+}
+
+void GaussianObjectModel::AddContact(Contact contact)
+{
+ ImplicitObjectModel::AddContact(contact);
+}
+
+
+
+void GaussianObjectModel::Update()
+{
+ gpModel->Calculate(CreateSamples(),noise);
+}
+
+std::vector<float> GaussianObjectModel::GetContactWeights()
+{
+ std::vector<float> result;
+ for (size_t i = 0; i < contacts->size(); ++i) {
+ result.push_back(1.f);
+ }
+ return result;
+}
+
+std::vector<DataR3R1> GaussianObjectModel::CreateSamples()
+{
+ std::vector<DataR3R1> samples;
+ for(Contact c: *contacts) {
+ samples.push_back(DataR3R1(c.Position(),0));
+ samples.push_back(DataR3R1(c.Position()- c.Normal()*0.05,-1));
+ samples.push_back(DataR3R1(c.Position()+ c.Normal()*0.05,+1));
+ }
+ return samples;
+}
diff --git a/VirtualRobot/math/GaussianObjectModel.h b/VirtualRobot/math/GaussianObjectModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..564716ae6f0289d566c2527e2c4a925a2182a034
--- /dev/null
+++ b/VirtualRobot/math/GaussianObjectModel.h
@@ -0,0 +1,53 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_GaussianObjectModel
+#define math_GaussianObjectModel
+
+#include "MathForwardDefinitions.h"
+
+#include "Contact.h"
+#include "DataR3R1.h"
+#include "GaussianImplicitSurface3D.h"
+#include "ImplicitObjectModel.h"
+
+namespace math
+{
+
+ class GaussianObjectModel :
+ public ImplicitObjectModel
+ {
+ public:
+ float Noise(){ return noise;}
+ GaussianObjectModel(float noise);
+ void AddContact(Contact contact);
+ void Update() override;
+ std::vector<float> GetContactWeights() override;
+
+
+ private:
+ float noise;
+ GaussianImplicitSurface3DPtr gpModel;
+ std::vector<DataR3R1> CreateSamples();
+
+ };
+}
+
+#endif // math_GaussianObjectModel
diff --git a/VirtualRobot/math/GaussianObjectModelNormals.cpp b/VirtualRobot/math/GaussianObjectModelNormals.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98f73df557d5a9b998507454c4046c76cbeb1ab2
--- /dev/null
+++ b/VirtualRobot/math/GaussianObjectModelNormals.cpp
@@ -0,0 +1,57 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include <iostream>
+
+#include "GaussianObjectModelNormals.h"
+#include "MathForwardDefinitions.h"
+
+using namespace math;
+
+GaussianObjectModelNormals::GaussianObjectModelNormals(float noise, float normalNoise, float normalScale)
+ : noise(noise), normalNoise(normalNoise), normalScale(normalScale)
+{
+ model = gpModel = GaussianImplicitSurface3DNormalsPtr(new GaussianImplicitSurface3DNormals());
+}
+void GaussianObjectModelNormals::AddContact(Contact contact)
+{
+ //std::cout<<"addContact"<<std::endl;
+ ImplicitObjectModel::AddContact(contact);
+}
+void GaussianObjectModelNormals::Update()
+{
+ std::cout<<"contacts"<<contacts->size()<<std::endl;
+ if(contacts->size()==1){
+ Contact c = contacts->at(0);
+ Eigen::Vector3f dir1, dir2;
+ contacts->push_back(Contact(c.Position()+Helpers::GetOrthonormalVectors(c.Normal(),dir1,dir2),c.Normal()));
+ }
+ gpModel->Calculate(*contacts,noise,normalNoise,normalScale);
+}
+
+std::vector<float> GaussianObjectModelNormals::GetContactWeights()
+{
+ std::vector<float> result;
+ for (size_t i = 0; i < contacts->size(); ++i) {
+ result.push_back(1.f);
+ }
+ return result;
+}
+
diff --git a/VirtualRobot/math/GaussianObjectModelNormals.h b/VirtualRobot/math/GaussianObjectModelNormals.h
new file mode 100644
index 0000000000000000000000000000000000000000..524713d28914ae16fee438e72c2fb7e1a593d65a
--- /dev/null
+++ b/VirtualRobot/math/GaussianObjectModelNormals.h
@@ -0,0 +1,54 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_GaussianObjectModelNormals
+#define math_GaussianObjectModelNormals
+
+#include "MathForwardDefinitions.h"
+
+#include "Contact.h"
+#include "GaussianImplicitSurface3DNormals.h"
+#include "ImplicitObjectModel.h"
+
+namespace math
+{
+
+ class GaussianObjectModelNormals :
+ public ImplicitObjectModel
+ {
+ public:
+ float Noise(){ return noise;}
+ GaussianObjectModelNormals(float noise, float normalNoise, float normalScale);
+ void AddContact(Contact contact);
+ void Update() override;
+ std::vector<float> GetContactWeights() override;
+
+
+ private:
+ float noise;
+ float normalNoise;
+ float normalScale;
+ GaussianImplicitSurface3DNormalsPtr gpModel;
+ //ContactList contacts;
+
+ };
+}
+
+#endif // math_GaussianObjectModelNormals
diff --git a/VirtualRobot/math/Grid3D.cpp b/VirtualRobot/math/Grid3D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39612f2b79253a7ce1b52b5d620a691348f429f4
--- /dev/null
+++ b/VirtualRobot/math/Grid3D.cpp
@@ -0,0 +1,64 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Grid3D.h"
+#include "cmath"
+#include "Helpers.h"
+
+using namespace math;
+
+
+Grid3D::Grid3D(Eigen::Vector3f p1, Eigen::Vector3f p2, int stepsX, int stepsY, int stepsZ)
+{
+ this->p1 = p1;
+ this->p2 = p2;
+ this->stepsX = stepsX;
+ this->stepsY = stepsY;
+ this->stepsZ = stepsZ;
+}
+
+Grid3DPtr Grid3D::CreateFromBox(Eigen::Vector3f p1, Eigen::Vector3f p2, float stepLength)
+{
+ Eigen::Vector3f steps = (p2 - p1) / stepLength;
+ return Grid3DPtr(new Grid3D(p1, p2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
+}
+
+
+Eigen::Vector3f Grid3D::Get(int x, int y, int z){
+ return Eigen::Vector3f(Helpers::Lerp(p1.x(), p2.x(), 0, stepsX, x),
+ Helpers::Lerp(p1.y(), p2.y(), 0, stepsY, y),
+ Helpers::Lerp(p1.z(), p2.z(), 0, stepsZ, z));
+}
+
+std::vector<Eigen::Vector3f> Grid3D::AllGridPoints()
+{
+ std::vector<Eigen::Vector3f> points;
+ for (int x = 0; x <= stepsX; x++)
+ {
+ for (int y = 0; y <= stepsY; y++)
+ {
+ for (int z = 0; z <= stepsZ; z++)
+ {
+ points.push_back(Get(x, y, z));
+ }
+ }
+ }
+ return points;
+}
diff --git a/VirtualRobot/math/Grid3D.h b/VirtualRobot/math/Grid3D.h
new file mode 100644
index 0000000000000000000000000000000000000000..70ede07404774b4bcd78b14df8c5245c00e4ccd5
--- /dev/null
+++ b/VirtualRobot/math/Grid3D.h
@@ -0,0 +1,53 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Grid3D
+#define math_Grid3D
+
+#include "MathForwardDefinitions.h"
+
+namespace math
+{
+ class Grid3D
+ {
+ public:
+ Eigen::Vector3f P1() { return p1; }
+ Eigen::Vector3f P2() { return p2; }
+ int StepsX() { return stepsX; }
+ int StepsY() { return stepsY; }
+ int StepsZ() { return stepsZ; }
+
+ Grid3D(Eigen::Vector3f p1, Eigen::Vector3f p2, int stepsX, int stepsY, int stepsZ);
+ static Grid3DPtr CreateFromBox(Eigen::Vector3f p1, Eigen::Vector3f p2, float stepLength);
+
+ Eigen::Vector3f Get(int x, int y, int z);
+ std::vector<Eigen::Vector3f> AllGridPoints();
+
+ private :
+ Eigen::Vector3f p1;
+ Eigen::Vector3f p2;
+ int stepsX;
+ int stepsY;
+ int stepsZ;
+
+ };
+}
+
+#endif // math_Grid3D
diff --git a/VirtualRobot/math/GridCacheFloat3.cpp b/VirtualRobot/math/GridCacheFloat3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fe88d659c429d1aebab1f5b766cf1e602a0597f
--- /dev/null
+++ b/VirtualRobot/math/GridCacheFloat3.cpp
@@ -0,0 +1,62 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "GridCacheFloat3.h"
+#include "Array3D.h"
+
+using namespace math;
+
+
+
+GridCacheFloat3::GridCacheFloat3(int size, std::function<float (Index3)>& getData)
+{
+ this->size = size;
+ this->getData = getData;
+ data = Array3DFloatPtr(new Array3D<float>(size));
+ valid = Array3DBoolPtr(new Array3D<bool>(size));
+
+ for (int x = 0; x < size; x++)
+ {
+ for (int y = 0; y < size; y++)
+ {
+ for (int z = 0; z < size; z++)
+ {
+ valid->Set(x,y,z, false);
+ }
+ }
+ }
+
+}
+
+
+float GridCacheFloat3::Get(int x, int y, int z)
+{
+ if (valid->Get(x,y,z))
+ {
+ return data->Get(x,y,z);
+ }
+ else
+ {
+ float val = getData(Index3(x, y, z));
+ data->Set(x,y,z,val);
+ valid->Set(x,y,z, true);
+ return val;
+ }
+}
diff --git a/VirtualRobot/math/GridCacheFloat3.h b/VirtualRobot/math/GridCacheFloat3.h
new file mode 100644
index 0000000000000000000000000000000000000000..a77c13b324f20d2d15fe59dff0d6e6670bdc8379
--- /dev/null
+++ b/VirtualRobot/math/GridCacheFloat3.h
@@ -0,0 +1,46 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_GridCacheFloat3
+#define math_GridCacheFloat3
+
+#include "MathForwardDefinitions.h"
+
+namespace math
+{
+
+class GridCacheFloat3
+{
+public:
+
+ GridCacheFloat3(int size, std::function<float(Index3)> &getData);
+
+ float Get(int x, int y, int z);
+
+
+ private:
+ Array3DFloatPtr data;
+ Array3DBoolPtr valid;
+ int size;
+ std::function<float(Index3)> getData;
+ };
+}
+
+#endif // math_GridCacheFloat3
diff --git a/VirtualRobot/math/Helpers.cpp b/VirtualRobot/math/Helpers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec4acfc2816847e05641ff737a92102df3135382
--- /dev/null
+++ b/VirtualRobot/math/Helpers.cpp
@@ -0,0 +1,186 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Helpers.h"
+#include <stdexcept>
+using namespace math;
+
+
+Eigen::Vector3f math::Helpers::GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f &dir1, Eigen::Vector3f &dir2)
+{
+ vec = vec.normalized();
+ dir1 = vec.cross(Eigen::Vector3f(0,0,1));
+ if (dir1.norm() < 0.1f)
+ {
+ dir1 = vec.cross(Eigen::Vector3f(0,1,0));
+ }
+ dir1 = -dir1.normalized();
+ dir2 = vec.cross(dir1).normalized();
+ return vec;
+}
+
+float Helpers::ShiftAngle0_2PI(float a)
+{
+ while (a < 0) a += 2*M_PI;
+ while (a > 2*M_PI) a -= 2*M_PI;
+ return a;
+}
+
+void Helpers::GetIndex(float t, float minT, float maxT, int count, int &i, float &f)
+{
+ if (minT == maxT)
+ {
+ f = i = 0;
+ return;
+ }
+ f = ((t - minT) / (maxT - minT)) * (count - 1);
+ i = std::max(0, std::min(count - 2, (int)f));
+ f -= i;
+}
+
+float Helpers::FastDistribution(float x, float sigma2)
+{
+ return std::exp(-x*x /2 /sigma2);
+}
+
+float Helpers::Clamp(float min, float max, float value)
+{
+ if (value < min) return min;
+ if (value > max) return max;
+ return value;
+}
+
+float Helpers::Lerp(float a, float b, float f)
+{
+ return a * (1 - f) + b * f;
+}
+
+Eigen::Vector3f Helpers::Lerp(const Eigen::Vector3f &a, const Eigen::Vector3f &b, float f)
+{
+ return Eigen::Vector3f(Lerp(a(0), b(0), f),
+ Lerp(a(1), b(1), f),
+ Lerp(a(2), b(2), f));
+}
+
+Eigen::Quaternionf Helpers::Lerp(const Eigen::Quaternionf &a, const Eigen::Quaternionf &b, float f)
+{
+ return a.slerp(f, b);
+}
+
+float Helpers::Lerp(float a, float b, int min, int max, int val)
+{
+ if (min == max) return a; //TODO
+ return Lerp(a, b, (val - min) / (float)(max - min));
+}
+
+float Helpers::Angle(Eigen::Vector2f v) { return (float)std::atan2(v.y(), v.x());}
+
+int Helpers::Sign(float x){
+ return x > 0 ? 1 : (x == 0 ? 0 : -1);
+}
+
+void Helpers::AssertNormalized(Eigen::Vector3f vec, float epsilon)
+{
+ float len = vec.squaredNorm();
+ if (len < 1 - epsilon || len > 1 + epsilon) throw std::runtime_error("Vector is not normalized");
+}
+
+std::vector<float> Helpers::FloatRange(float start, float end, int steps)
+{
+ std::vector<float> result;
+ for (int i = 0; i < steps+1; ++i) {
+ result.push_back(Lerp(start, end, i/(float) steps));
+ }
+ return result;
+}
+
+std::vector<Eigen::Vector3f> Helpers::VectorRangeSymmetric(float start, float end, int steps)
+{
+ std::vector<float> vals = FloatRange(start, end, steps);
+ return VectorRange(vals, vals, vals);
+}
+
+std::vector<Eigen::Vector3f> Helpers::VectorRange(std::vector<float> xvals, std::vector<float> yvals, std::vector<float> zvals)
+{
+ std::vector<Eigen::Vector3f> result;
+ for (float x : xvals)
+ {
+ for (float y : yvals)
+ {
+ for (float z : zvals)
+ {
+ result.push_back(Eigen::Vector3f(x, y, z));
+ }
+ }
+ }
+ return result;
+}
+
+float Helpers::SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b)
+{
+ return (float)std::atan2(a.cross(b).norm(), a.dot(b));
+}
+
+Eigen::Vector3f Helpers::CwiseMin(Eigen::Vector3f a, Eigen::Vector3f b)
+{
+ return Eigen::Vector3f(std::min(a.x(), b.x()),
+ std::min(a.y(), b.y()),
+ std::min(a.z(), b.z()));
+}
+
+Eigen::Vector3f Helpers::CwiseMax(Eigen::Vector3f a, Eigen::Vector3f b)
+{
+ return Eigen::Vector3f(std::max(a.x(), b.x()),
+ std::max(a.y(), b.y()),
+ std::max(a.z(), b.z()));
+
+}
+
+Eigen::Vector3f Helpers::CwiseDivide(Eigen::Vector3f a, Eigen::Vector3f b)
+{
+ return Eigen::Vector3f(a.x()/ b.x(),
+ a.y()/ b.y(),
+ a.z()/ b.z());
+}
+
+Eigen::Vector3f Helpers::Average(std::vector<Eigen::Vector3f> vectors)
+{
+ Eigen::Vector3f avg = Eigen::Vector3f(0,0,0);
+ if(vectors.size() == 0) return avg;
+ for(Eigen::Vector3f v : vectors){
+ avg += v;
+ }
+ avg /= vectors.size();
+ return avg;
+}
+void Helpers::Swap(float &a,float &b)
+{
+ float t = a;
+ a = b;
+ b = t;
+}
+
+Eigen::Matrix4f Helpers::CreatePose(const Eigen::Vector3f &pos, const Eigen::Quaternionf &ori)
+{
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ pose.block<3,3>(0,0) = ori.toRotationMatrix();
+ pose.block<3,1>(0,3) = pos;
+ return pose;
+}
diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..3610552043ecd50c06734785591638f4df3b9eea
--- /dev/null
+++ b/VirtualRobot/math/Helpers.h
@@ -0,0 +1,60 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Helpers
+#define math_Helpers
+
+#include "MathForwardDefinitions.h"
+
+namespace math
+{
+ class Helpers
+ {
+ public:
+
+
+ static Eigen::Vector3f GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2);
+ static float ShiftAngle0_2PI(float a);
+ static void GetIndex(float t, float minT, float maxT, int count, int& i, float& f);
+ static float FastDistribution(float x, float sigma2);
+ static float Clamp(float min, float max, float value);
+ static float Lerp(float a, float b, float f);
+ static Eigen::Vector3f Lerp(const Eigen::Vector3f& a, const Eigen::Vector3f& b, float f);
+ static Eigen::Quaternionf Lerp(const Eigen::Quaternionf& a, const Eigen::Quaternionf& b, float f);
+ static float Lerp(float a, float b, int min, int max, int val);
+ static float Angle(Eigen::Vector2f v);
+ static int Sign(float x);
+ static void AssertNormalized(Eigen::Vector3f vec, float epsilon = 0.05f);
+ static std::vector<float> FloatRange(float start, float end, int steps);
+ static std::vector<Eigen::Vector3f> VectorRangeSymmetric(float start, float end, int steps);
+ static std::vector<Eigen::Vector3f> VectorRange(std::vector<float> xvals, std::vector<float> yvals, std::vector<float> zvals);
+ static float SmallestAngle(Eigen::Vector3f a, Eigen::Vector3f b);
+ static Eigen::Vector3f CwiseMin(Eigen::Vector3f a, Eigen::Vector3f b);
+ static Eigen::Vector3f CwiseMax(Eigen::Vector3f a, Eigen::Vector3f b);
+ static Eigen::Vector3f CwiseDivide(Eigen::Vector3f a, Eigen::Vector3f b);
+ static Eigen::Vector3f Average(std::vector<Eigen::Vector3f> vectors);
+ static void Swap(float &a,float &b);
+ static Eigen::Matrix4f CreatePose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori);
+
+ private:
+ };
+}
+
+#endif // math_Helpers
diff --git a/VirtualRobot/math/ImplicitObjectModel.cpp b/VirtualRobot/math/ImplicitObjectModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3fe8af15eec59f9e0a612f4203b4e9f0a768967e
--- /dev/null
+++ b/VirtualRobot/math/ImplicitObjectModel.cpp
@@ -0,0 +1,44 @@
+/*
+* This file is part of math.
+*
+* math is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* math is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "ImplicitObjectModel.h"
+
+using namespace math;
+
+
+ImplicitObjectModel::ImplicitObjectModel()
+{
+ contacts = ContactListPtr(new ContactList());
+}
+
+
+void ImplicitObjectModel::AddContact(Contact contact)
+{
+ contacts->push_back(contact);
+ Update();
+}
+
+
+void ImplicitObjectModel::Clear()
+{
+ contacts->clear();
+ //Update();
+}
+
diff --git a/VirtualRobot/math/ImplicitObjectModel.h b/VirtualRobot/math/ImplicitObjectModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3788290cd68b45774902e8a6c5b8899212b5e54
--- /dev/null
+++ b/VirtualRobot/math/ImplicitObjectModel.h
@@ -0,0 +1,50 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_ImplicitObjectModel
+#define math_ImplicitObjectModel
+
+#include "MathForwardDefinitions.h"
+#include "ContactList.h"
+#include "SimpleAbstractFunctionR3R1.h"
+
+namespace math
+{
+class ImplicitObjectModel
+{
+public:
+ ContactListPtr Contacts() {return contacts;}
+ SimpleAbstractFunctionR3R1Ptr Model() {return model;}
+
+ ImplicitObjectModel();
+ virtual void Update() = 0;
+ virtual void AddContact(Contact contact) = 0;
+ //virtual void RemoveContact(Contact contact);
+ virtual void Clear();
+ virtual std::vector<float> GetContactWeights() = 0;
+
+
+protected:
+ ContactListPtr contacts;
+ SimpleAbstractFunctionR3R1Ptr model;
+};
+}
+
+#endif // math_ImplicitObjectModel
diff --git a/VirtualRobot/math/ImplicitPlane.cpp b/VirtualRobot/math/ImplicitPlane.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..032a590dc8fd8a99b6db1667a0fae31f448d09ce
--- /dev/null
+++ b/VirtualRobot/math/ImplicitPlane.cpp
@@ -0,0 +1,92 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "ImplicitPlane.h"
+#include "Contact.h"
+#include "Plane.h"
+using namespace math;
+
+
+ImplicitPlane::ImplicitPlane(float a, float b, float c, float d)
+ : a(a), b(b), c(c), d(d)
+{
+}
+
+ImplicitPlane ImplicitPlane::Normalize()
+{
+ float len = GetNormal().norm();
+ return ImplicitPlane(a / len, b / len, c / len, d / len);
+}
+
+ImplicitPlane ImplicitPlane::Flipped()
+{
+ return ImplicitPlane(-a, -b, -c, -d);
+}
+
+Eigen::Vector3f ImplicitPlane::GetNormal()
+{
+ return Eigen::Vector3f(a, b, c);
+}
+
+Eigen::Vector3f ImplicitPlane::GetClosestPoint(Eigen::Vector3f v)
+{
+ Eigen::Vector3f normal = GetNormal();
+ float d = this->d - v.dot(normal);
+ float denominator = a * a + b * b + c * c;
+ return normal * d / denominator + v;
+
+}
+
+ImplicitPlane ImplicitPlane::FromPositionNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
+{
+ return ImplicitPlane(normal.x(), normal.y(), normal.z(), normal.dot(pos));
+
+}
+
+ImplicitPlane ImplicitPlane::FromContact(Contact c)
+{
+ return FromPositionNormal(c.Position(), c.Normal());
+}
+
+Line ImplicitPlane::Intersect(Plane plane)
+{
+
+ Eigen::Vector3f n = GetNormal();
+ Eigen::Vector3f p = plane.Pos();
+ Eigen::Vector3f u = plane.Dir1();
+ Eigen::Vector3f v = plane.Dir2();
+ if (std::abs(n.dot(u)) < std::abs(n.dot(v)))
+ {
+ Eigen::Vector3f tmp = u;
+ u = v;
+ v = tmp;
+ }
+
+ Eigen::Vector3f pos = p + (d - n.dot(p)) / n.dot(u) * u;
+ Eigen::Vector3f dir = v - n.dot(v) / n.dot(u) * u;
+ return Line(pos, dir);
+
+}
+
+
+float ImplicitPlane::Get(Eigen::Vector3f pos)
+{
+ return GetNormal().dot(pos - GetClosestPoint(pos));
+}
diff --git a/VirtualRobot/math/ImplicitPlane.h b/VirtualRobot/math/ImplicitPlane.h
new file mode 100644
index 0000000000000000000000000000000000000000..3342d6140eede4d2062212d46ce4be254c236cca
--- /dev/null
+++ b/VirtualRobot/math/ImplicitPlane.h
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_ImplicitPlane
+#define math_HapticExplorationLibrary_ImplicitPlane
+
+#include "MathForwardDefinitions.h"
+
+#include "AbstractFunctionR3R1.h"
+
+
+
+
+namespace math
+{
+
+class ImplicitPlane
+ : public SimpleAbstractFunctionR3R1
+{
+public:
+ // ax + by + cz = d
+ ImplicitPlane(float a, float b, float c, float d);
+
+ float A() { return a; }
+ float B() { return b; }
+ float C() { return c; }
+ float D() { return d; }
+
+ ImplicitPlane Normalize();
+ ImplicitPlane Flipped();
+ Eigen::Vector3f GetNormal();
+ Eigen::Vector3f GetClosestPoint(Eigen::Vector3f v);
+ static ImplicitPlane FromPositionNormal(Eigen::Vector3f pos, Eigen::Vector3f normal);
+ static ImplicitPlane FromContact(Contact c);
+
+ // https://de.wikipedia.org/wiki/Schnittgerade
+ Line Intersect(Plane plane);
+
+ float Get(Eigen::Vector3f pos);
+
+private:
+ float a;
+ float b;
+ float c;
+ float d;
+
+};
+}
+
+#endif // math_HapticExplorationLibrary_ImplicitPlane
diff --git a/VirtualRobot/math/Index3.cpp b/VirtualRobot/math/Index3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32873b0f3f9d34e23dbb379daee68e4716c49806
--- /dev/null
+++ b/VirtualRobot/math/Index3.cpp
@@ -0,0 +1,37 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Index3.h"
+
+using namespace math;
+
+
+Index3::Index3(int x, int y, int z)
+ : x(x), y(y), z(z)
+{
+}
+
+std::string Index3::ToString()
+{
+ std::stringstream ss;
+ ss << x << " " << y << " " << " " << z;
+ return ss.str();
+}
+
diff --git a/VirtualRobot/math/Index3.h b/VirtualRobot/math/Index3.h
new file mode 100644
index 0000000000000000000000000000000000000000..02f3c8d1e7be1b2081bb8d1e2998f98719aaabb2
--- /dev/null
+++ b/VirtualRobot/math/Index3.h
@@ -0,0 +1,63 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_Index3
+#define math_HapticExplorationLibrary_Index3
+
+#include "MathForwardDefinitions.h"
+
+
+
+
+
+namespace math
+{
+
+struct Index3
+{
+private:
+ int x;
+ int y;
+ int z;
+
+public :
+ int X() { return x; }
+ int Y() { return y;}
+ int Z() { return z; }
+ void SetX(int value){ x = value; }
+ void SetY(int value){ y = value; }
+ void SetZ(int value){ z = value; }
+
+ Index3(int x, int y, int z);
+
+std::string ToString();
+
+};
+
+/*static Index3 operator -(Index3 a);
+static Index3 operator +(Index3 a, Index3 b);
+static Index3 operator -(Index3 a, Index3 b);
+static Index3 operator *(Index3 a, int f);
+static Index3 operator *(int f, Index3 a);
+static Index3 operator /(Index3 a, int f);*/
+
+}
+
+#endif // math_HapticExplorationLibrary_Index3
diff --git a/VirtualRobot/math/Line.cpp b/VirtualRobot/math/Line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7389946646ab4327373419f7e91be8da1d8c693
--- /dev/null
+++ b/VirtualRobot/math/Line.cpp
@@ -0,0 +1,147 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Line.h"
+#include "Triangle.h"
+#include "Primitive.h"
+#include "float.h"
+
+using namespace math;
+
+
+
+Line::Line(Eigen::Vector3f pos, Eigen::Vector3f dir)
+ : pos(pos), dir(dir)
+{
+}
+
+Line Line::Normalized()
+{
+ return Line(pos, dir.normalized());
+}
+
+Eigen::Vector3f Line::Get(float t)
+{
+ return pos + t * dir;
+}
+
+Eigen::Vector3f Line::GetDerivative(float)
+{
+ return dir;
+}
+
+Eigen::Vector3f Line::GetClosestPoint(Eigen::Vector3f p)
+{
+ return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
+}
+
+float Line::GetT(Eigen::Vector3f p)
+{
+ return (p - pos).dot(dir) / dir.squaredNorm();
+
+}
+
+std::string Line::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir << ")";
+ return ss.str();
+}
+
+
+//https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+bool Line::IntersectsTriangle(Triangle tri, float &out)
+{
+ const float EPS = 0.000; //TODO
+ Eigen::Vector3f e1, e2; //Edge1, Edge2
+ Eigen::Vector3f P, Q, T;
+ float det, inv_det, u, v;
+ float t;
+ Eigen::Vector3f V1 = tri.P1();
+ Eigen::Vector3f V2 = tri.P2();
+ Eigen::Vector3f V3 = tri.P3();
+
+ //Find vectors for two edges sharing V1
+ e1 = V2 -V1;
+ e2 = V3 -V1;
+
+ //Begin calculating determinant - also used to calculate u parameter
+ P = dir.cross(e2);
+ //if determinant is near zero, ray lies in plane of triangle
+ det =e1.dot( P);
+ //NOT CULLING
+ if(det > -EPS && det < EPS) return 0;
+ inv_det = 1.f / det;
+
+ //calculate distance from V1 to ray origin
+ T = pos - V1;
+
+ //Calculate u parameter and test bound
+ u = T.dot(P) * inv_det;
+ //The intersection lies outside of the triangle
+ if(u < 0.f || u > 1.f) return 0;
+
+ //Prepare to test v parameter
+ Q = T.cross(e1);
+
+ //Calculate V parameter and test bound
+ v = dir.dot(Q) * inv_det;
+ //The intersection lies outside of the triangle
+ if(v < 0.f || u + v > 1.f) return 0;
+
+ t = e2.dot(Q) * inv_det;
+
+ if(t > EPS) { //ray intersection
+ out = t;
+ return 1;
+ }
+
+ // No hit, no win
+ return 0;
+}
+
+bool Line::IntersectsPrimitive(PrimitivePtr p, float &out)
+{
+ float min = FLT_MAX;
+ float t;
+ for(Triangle tri : *p){
+ if (IntersectsTriangle(tri, t)){
+ if(t<min) min = t;
+ }
+ }
+ out = min;
+ return out < FLT_MAX;
+}
+
+Line Line::FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2)
+{
+ return Line(p1, p2 - p1);
+}
+
+Line Line::FromPoses(const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2)
+{
+ return FromPoints(p1.block<3,1>(0,3), p2.block<3,1>(0,3));
+}
+
+
+
+
+
+
diff --git a/VirtualRobot/math/Line.h b/VirtualRobot/math/Line.h
new file mode 100644
index 0000000000000000000000000000000000000000..40bc6951226ddb537a780a3d7a61327c289a60d3
--- /dev/null
+++ b/VirtualRobot/math/Line.h
@@ -0,0 +1,56 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_Line
+#define math_HapticExplorationLibrary_Line
+
+#include "MathForwardDefinitions.h"
+
+
+namespace math
+{
+
+ class Line
+ {
+ public:
+ Line(Eigen::Vector3f pos, Eigen::Vector3f dir);
+ Eigen::Vector3f Pos(){return pos;}
+ Eigen::Vector3f Dir(){return dir;}
+
+ Line Normalized();
+ Eigen::Vector3f Get(float t);
+ Eigen::Vector3f GetDerivative(float t);
+ Eigen::Vector3f GetClosestPoint(Eigen::Vector3f p);
+ float GetT(Eigen::Vector3f p);
+ std::string ToString();
+
+ bool IntersectsTriangle(Triangle tri, float& t);
+ bool IntersectsPrimitive(PrimitivePtr p, float& t);
+
+ static Line FromPoints(Eigen::Vector3f p1, Eigen::Vector3f p2);
+ static Line FromPoses(const Eigen::Matrix4f& p1, const Eigen::Matrix4f& p2);
+
+ private:
+ Eigen::Vector3f pos;
+ Eigen::Vector3f dir;
+ };
+}
+
+#endif // math_HapticExplorationLibrary_Line
diff --git a/VirtualRobot/math/LineR2.cpp b/VirtualRobot/math/LineR2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a8795ec0dd8b3397a76e3100b81c88734f97a7be
--- /dev/null
+++ b/VirtualRobot/math/LineR2.cpp
@@ -0,0 +1,83 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "LineR2.h"
+
+
+using namespace math;
+
+
+
+LineR2::LineR2(Eigen::Vector3f pos, Eigen::Vector3f dir)
+ : pos(pos), dir(dir)
+{
+}
+
+LineR2 LineR2::Normalized()
+{
+ return LineR2(pos,dir.normalized());
+}
+
+Eigen::Vector3f LineR2::Get(float t)
+{
+ return pos + t * dir;
+}
+
+Eigen::Vector3f LineR2::GetDerivative(float)
+{
+ return dir;
+}
+
+Eigen::Vector3f LineR2::GetClosestPoint(Eigen::Vector3f p)
+{
+ return pos - (pos - p).dot(dir) * dir / dir.squaredNorm();
+}
+
+float LineR2::GetT(Eigen::Vector3f p)
+{
+ return (p - pos).dot(dir) / dir.squaredNorm();
+
+}
+
+std::string LineR2::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir << ")";
+ return ss.str();
+
+}
+
+//bool LineR2::Intersect(const Eigen::Vector3f &pos1, const Eigen::Vector3f &dir1, const Eigen::Vector3f &pos2, const Eigen::Vector3f &dir2, float &t, float &u)
+//{
+//
+//}
+//
+//Vec3Opt LineR2::Intersect(const LineR2& l2)
+//{
+// float t, u;
+// if (Intersect(this, l2, &t, &u))
+// {
+// return Vec3Opt(Get(t));
+// }
+// else
+// {
+// return Vec3Opt();
+// }
+//}
diff --git a/VirtualRobot/math/LineR2.h b/VirtualRobot/math/LineR2.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e04ff86e21110e1079a0de26971e7d82002d8d6
--- /dev/null
+++ b/VirtualRobot/math/LineR2.h
@@ -0,0 +1,60 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_LineR2
+#define math_HapticExplorationLibrary_LineR2
+
+#include "MathForwardDefinitions.h"
+
+
+
+
+
+namespace math
+{
+
+class LineR2
+{
+public:
+ LineR2(Eigen::Vector3f pos, Eigen::Vector3f dir);
+
+ Eigen::Vector3f Pos(){return pos;}
+ Eigen::Vector3f Dir(){return dir;}
+
+ LineR2 Normalized ();
+ Eigen::Vector3f Get(float t);
+ Eigen::Vector3f GetDerivative(float t);
+ Eigen::Vector3f GetClosestPoint(Eigen::Vector3f p);
+ float GetT(Eigen::Vector3f p);
+ std::string ToString();
+ //Vec3Opt Intersect(const LineR2& l2);
+ //static bool Intersect(const Eigen::Vector3f& pos1, const Eigen::Vector3f& dir1, const Eigen::Vector3f& pos2, const Eigen::Vector3f& dir2, float& t,float& u);
+
+ //static bool Intersect(LineR2 l1, LineR2 l2, out float t, out float u);
+
+
+private:
+ Eigen::Vector3f pos;
+ Eigen::Vector3f dir;
+
+};
+}
+
+#endif // math_HapticExplorationLibrary_LineR2
diff --git a/VirtualRobot/math/LineStrip.cpp b/VirtualRobot/math/LineStrip.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4734a6f270c6c2e1bb157ba99a31c056b824f21d
--- /dev/null
+++ b/VirtualRobot/math/LineStrip.cpp
@@ -0,0 +1,52 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "LineStrip.h"
+#include "Helpers.h"
+
+using namespace math;
+
+
+
+LineStrip::LineStrip(Vec3ListPtr points, float minT, float maxT)
+{
+ this->points = points;
+ this->minT = minT;
+ this->maxT = maxT;
+}
+
+
+Eigen::Vector3f LineStrip::Get(float t)
+{
+ int i; float f;
+ GetIndex(t, i, f);
+ return points->at(i) * (1 - f) + points->at(i+1) * f;
+}
+
+Eigen::Vector3f LineStrip::GetDirection(int i)
+{
+ return points->at(i+1) - points->at(i);
+}
+
+void LineStrip::GetIndex(float t, int &i, float &f)
+{
+ Helpers::GetIndex(t, minT, maxT, points->size(), i, f);
+}
+
diff --git a/VirtualRobot/math/LineStrip.h b/VirtualRobot/math/LineStrip.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e85e4b6af951c469a4fb0c0ebf5bf970fe75fff
--- /dev/null
+++ b/VirtualRobot/math/LineStrip.h
@@ -0,0 +1,52 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_LineStrip
+#define math_LineStrip
+#include "MathForwardDefinitions.h"
+
+#include "SimpleAbstractFunctionR1R3.h"
+
+
+namespace math
+{
+
+ class LineStrip
+ : public SimpleAbstractFunctionR1R3
+ {
+ public:
+ Vec3ListPtr points;
+ float minT, maxT;
+
+ int Count() { return points->size(); }
+
+ LineStrip(Vec3ListPtr points, float minT, float maxT);
+
+ bool InLimits(float t);
+ Eigen::Vector3f Get(float t) override;
+
+ private:
+ Eigen::Vector3f GetDirection(int i);
+ void GetIndex(float t, int& i, float& f);
+
+ };
+}
+
+#endif // math_LineStrip
diff --git a/VirtualRobot/math/LinearContinuedBezier.cpp b/VirtualRobot/math/LinearContinuedBezier.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..36f5efdb12c73189957da42e36b87aad3cae8ada
--- /dev/null
+++ b/VirtualRobot/math/LinearContinuedBezier.cpp
@@ -0,0 +1,42 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "LinearContinuedBezier.h"
+
+using namespace math;
+
+math::LinearContinuedBezier::LinearContinuedBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
+ : p0(p0), p1(p1), p2(p2), p3(p3)
+{
+}
+
+Eigen::Vector3f math::LinearContinuedBezier::Get(float t)
+{
+ if (0 <= t && t <= 1) return Bezier::CubicBezierPoint(p0, p1, p2, p3, t);
+ if (t < 0) return p0 + (p0 - p1) * 3 * -t;
+ return p3 + (p3 - p2) * 3 * (t - 1);
+}
+
+Eigen::Vector3f math::LinearContinuedBezier::GetDerivative(float t)
+{
+ if (0 <= t && t <= 1) return Bezier::CubicBezierDerivative(p0, p1, p2, p3, t);
+ if (t < 0) return (p0 - p1) * -3;
+ return (p3 - p2) * 3;
+}
diff --git a/VirtualRobot/math/LinearContinuedBezier.h b/VirtualRobot/math/LinearContinuedBezier.h
new file mode 100644
index 0000000000000000000000000000000000000000..ecaacbfd4597b880c1c174ba7a2cb37953d3ed38
--- /dev/null
+++ b/VirtualRobot/math/LinearContinuedBezier.h
@@ -0,0 +1,54 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_LinearContinuedBezier
+#define math_LinearContinuedBezier
+
+#include "MathForwardDefinitions.h"
+#include "Bezier.h"
+#include "AbstractFunctionR1R3.h"
+
+namespace math
+{
+ class LinearContinuedBezier
+ : public AbstractFunctionR1R3
+ {
+ public:
+ LinearContinuedBezier(Eigen::Vector3f p0, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3);
+
+ Eigen::Vector3f P0() {return p0; }
+ Eigen::Vector3f P1() {return p1; }
+ Eigen::Vector3f P2() {return p2; }
+ Eigen::Vector3f P3() {return p3; }
+
+
+ Eigen::Vector3f Get(float t) override;
+ Eigen::Vector3f GetDerivative(float t) override;
+
+ private:
+ Eigen::Vector3f p0;
+ Eigen::Vector3f p1;
+ Eigen::Vector3f p2;
+ Eigen::Vector3f p3;
+
+ };
+}
+
+#endif // math_LinearContinuedBezier
diff --git a/VirtualRobot/math/MarchingCubes.cpp b/VirtualRobot/math/MarchingCubes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..75e7d29a81533960b21c2ac444e98efcb47cc289
--- /dev/null
+++ b/VirtualRobot/math/MarchingCubes.cpp
@@ -0,0 +1,575 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include <iostream>
+
+#include "Index3.h"
+#include "MarchingCubes.h"
+#include "Primitive.h"
+#include "stdexcept"
+#include "SimpleAbstractFunctionR3R1.h"
+#include "stdio.h"
+#include "GridCacheFloat3.h"
+
+using namespace math;
+
+
+static const float eps = 0.0001f;
+
+
+void MarchingCubes::Init(int size, Eigen::Vector3f /*center*/, Eigen::Vector3f /*start*/, int /*steps*/, float /*stepLength*/, GridCacheFloat3Ptr cache)
+{
+ _size = size;
+ _grids = Array3DGridCellPtr(new Array3D<GridCell>(size));
+ Gdata = cache;
+ for (int i = 0; i < size; i++)
+ for (int j = 0; j < size; j++)
+ for (int k = 0; k < size; k++)
+ {
+ GridCell cell = GridCell();
+
+ cell.P[0] = Eigen::Vector3f(i, j, k);
+ cell.P[1] = Eigen::Vector3f(i + 1, j, k);
+ cell.P[2] = Eigen::Vector3f(i + 1, j + 1, k);
+ cell.P[3] = Eigen::Vector3f(i, j + 1, k);
+ cell.P[4] = Eigen::Vector3f(i, j, k + 1);
+ cell.P[5] = Eigen::Vector3f(i + 1, j, k + 1);
+ cell.P[6] = Eigen::Vector3f(i + 1, j + 1, k + 1);
+ cell.P[7] = Eigen::Vector3f(i, j + 1, k + 1);
+
+ _grids->Set(i,j,k,cell);
+ }
+}
+
+PrimitivePtr MarchingCubes::Process(float isolevel)
+{
+ PrimitivePtr res = PrimitivePtr(new Primitive());
+ Process(isolevel, res); //TODO ref
+ return res;
+}
+
+void MarchingCubes::Process(float isolevel, PrimitivePtr primitive) //TODO ref?
+{
+ primitive->clear();
+ int triNum = 0;
+ for (int i = 0; i < _size; i++)
+ for (int j = 0; j < _size; j++)
+ for (int k = 0; k < _size; k++)
+ {
+ triNum = Polygonise(i, j, k, isolevel);
+ if (triNum > 0)
+ {
+ Build(primitive, triNum); //TODO ref
+ }
+ }
+ //if (primitive.CurrentVertex == 0) primitive.isDrawable = false;
+ //else primitive.InitializePrimitive(graphicsDevice);
+}
+
+PrimitivePtr MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, Index3 start)
+{
+ PrimitivePtr res = PrimitivePtr(new Primitive());
+ ProcessSingleSurfaceOptimized(isolevel, res, start); //TODO ref
+ return res;
+}
+
+//TODO do we use this?
+void MarchingCubes::ProcessSingleSurfaceOptimized(float isolevel, PrimitivePtr primitive, Index3 start) //TODO ref
+{
+ FringePtr fringe = FringePtr(new std::queue<Index3>);
+ fringe->push(start);
+ primitive->clear();
+ Array3DBoolPtr marked = Array3DBoolPtr(new Array3D<bool>(_size + 1));
+ marked->Set(start,true);
+ while (fringe->size() > 0)
+ {
+ Index3 index = fringe->front();
+ fringe->pop();
+ int i = index.X();
+ int j = index.Y();
+ int k = index.Z();
+ int triNum = Polygonise(i, j, k, isolevel);
+ if (triNum > 0)
+ {
+ Build(primitive, triNum);//TODO ref
+ }
+ if (triNum > 0 || fringe->size() == 0)
+ {
+ AddAndMark(i + 1, j, k, fringe, marked);
+ AddAndMark(i - 1, j, k, fringe, marked);
+ AddAndMark(i, j + 1, k, fringe, marked);
+ AddAndMark(i, j - 1, k, fringe, marked);
+ AddAndMark(i, j, k + 1, fringe, marked);
+ AddAndMark(i, j, k - 1, fringe, marked);
+ }
+ }
+}
+
+PrimitivePtr MarchingCubes::Calculate(Eigen::Vector3f center, Eigen::Vector3f start, int steps, float stepLength, SimpleAbstractFunctionR3R1Ptr modelPtr, float isolevel)
+{
+ int size = steps * 2;
+ start = (start - center) / stepLength;
+ Index3 startIndex = Index3((int)start.x() + steps, (int)start.y() + steps, (int)start.z() + steps);
+ MarchingCubes mc = MarchingCubes();
+ std::function<float(Index3)> getData = [steps, stepLength, center, modelPtr](Index3 index)
+ {
+ Eigen::Vector3f pos = Eigen::Vector3f((index.X() - steps) * stepLength + center.x(),
+ (index.Y() - steps) * stepLength + center.y(),
+ (index.Z() - steps) * stepLength + center.z());
+ return modelPtr->Get(pos);
+
+ };
+ mc.Init(size, center, start, steps, stepLength, GridCacheFloat3Ptr(new GridCacheFloat3(size+1, getData)));
+ PrimitivePtr triangles = mc.ProcessSingleSurfaceOptimized(isolevel, startIndex);
+
+ int oldSize = triangles->size();
+ int newSize;
+ PrimitivePtr result = PrimitivePtr(new Primitive());
+ for(Triangle t : *triangles){
+ if((t.P1()-t.P2()).squaredNorm() > eps &&
+ (t.P2()-t.P3()).squaredNorm() > eps &&
+ (t.P3()-t.P1()).squaredNorm() > eps)
+ {
+ result->push_back(t.Transform(- Eigen::Vector3f(1, 1, 1) * stepLength * steps + center, stepLength));
+ }
+ }
+ newSize = result->size();
+ std::cout << "MC deleted " << oldSize - newSize << " out of " << oldSize << " triangles" << std::endl; //TODO why do we throw away triangles here?
+ return result;
+}
+
+
+float MarchingCubes::GetVal(int x, int y, int z, int i)
+{
+ switch (i)
+ {
+ case 0:
+ return Gdata->Get(x, y, z);
+ case 1:
+ return Gdata->Get(x + 1, y, z);
+ case 2:
+ return Gdata->Get(x + 1, y + 1, z);
+ case 3:
+ return Gdata->Get(x, y + 1, z);
+ case 4:
+ return Gdata->Get(x, y, z + 1);
+ case 5:
+ return Gdata->Get(x + 1, y, z + 1);
+ case 6:
+ return Gdata->Get(x + 1, y + 1, z + 1);
+ case 7:
+ return Gdata->Get(x, y + 1, z + 1);
+ }
+ return 0;
+}
+
+Eigen::Vector3f MarchingCubes::GetPos(int x, int y, int z, int i)
+{
+ return _grids->Get(x, y, z).P[i];
+}
+
+void MarchingCubes::AddAndMark(int x, int y, int z, FringePtr fringe, Array3DBoolPtr marked)
+{
+ if (!IsValidIndex(x, y, z)) return;
+ if (marked->Get(x,y,z)) return;
+ fringe->push( Index3(x, y, z));
+ marked->Set(x,y,z,true);
+}
+
+bool MarchingCubes::IsValidIndex(int x, int y, int z)
+{
+ return x >= 0 && x < _size && y >= 0 && y < _size && z >= 0 && z < _size;
+}
+
+int MarchingCubes::Polygonise(int x, int y, int z, float isolevel)
+{
+ Eigen::Vector3f vertlist[12];
+ int i, ntriang = 0;
+ unsigned int cubeindex = 0; //TODO unsigned char == C# char?
+
+ if (GetVal(x, y, z, 0) > isolevel) cubeindex |= 1;
+ if (GetVal(x, y, z, 1) > isolevel) cubeindex |= 2;
+ if (GetVal(x, y, z, 2) > isolevel) cubeindex |= 4;
+ if (GetVal(x, y, z, 3) > isolevel) cubeindex |= 8;
+ if (GetVal(x, y, z, 4) > isolevel) cubeindex |= 16;
+ if (GetVal(x, y, z, 5) > isolevel) cubeindex |= 32;
+ if (GetVal(x, y, z, 6) > isolevel) cubeindex |= 64;
+ if (GetVal(x, y, z, 7) > isolevel) cubeindex |= 128;
+
+ /* Cube is entirely in/out of the surface */
+ if (_edgeTable[cubeindex] == 0)
+ return 0;
+
+ /* Find the vertices where the surface intersects the cube */
+ if ((_edgeTable[cubeindex] & 1) > 0)
+ vertlist[0] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 1), GetVal(x, y, z, 0), GetVal(x, y, z, 1));
+ if ((_edgeTable[cubeindex] & 2) > 0)
+ vertlist[1] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 2), GetVal(x, y, z, 1), GetVal(x, y, z, 2));
+ if ((_edgeTable[cubeindex] & 4) > 0)
+ vertlist[2] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 3), GetVal(x, y, z, 2), GetVal(x, y, z, 3));
+ if ((_edgeTable[cubeindex] & 8) > 0)
+ vertlist[3] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 0), GetVal(x, y, z, 3), GetVal(x, y, z, 0));
+ if ((_edgeTable[cubeindex] & 16) > 0)
+ vertlist[4] = VertexInterp(isolevel, GetPos(x, y, z, 4), GetPos(x, y, z, 5), GetVal(x, y, z, 4), GetVal(x, y, z, 5));
+ if ((_edgeTable[cubeindex] & 32) > 0)
+ vertlist[5] = VertexInterp(isolevel, GetPos(x, y, z, 5), GetPos(x, y, z, 6), GetVal(x, y, z, 5), GetVal(x, y, z, 6));
+ if ((_edgeTable[cubeindex] & 64) > 0)
+ vertlist[6] = VertexInterp(isolevel, GetPos(x, y, z, 6), GetPos(x, y, z, 7), GetVal(x, y, z, 6), GetVal(x, y, z, 7));
+ if ((_edgeTable[cubeindex] & 128) > 0)
+ vertlist[7] = VertexInterp(isolevel, GetPos(x, y, z, 7), GetPos(x, y, z, 4), GetVal(x, y, z, 7), GetVal(x, y, z, 4));
+ if ((_edgeTable[cubeindex] & 256) > 0)
+ vertlist[8] = VertexInterp(isolevel, GetPos(x, y, z, 0), GetPos(x, y, z, 4), GetVal(x, y, z, 0), GetVal(x, y, z, 4));
+ if ((_edgeTable[cubeindex] & 512) > 0)
+ vertlist[9] = VertexInterp(isolevel, GetPos(x, y, z, 1), GetPos(x, y, z, 5), GetVal(x, y, z, 1), GetVal(x, y, z, 5));
+ if ((_edgeTable[cubeindex] & 1024) > 0)
+ vertlist[10] = VertexInterp(isolevel, GetPos(x, y, z, 2), GetPos(x, y, z, 6), GetVal(x, y, z, 2), GetVal(x, y, z, 6));
+ if ((_edgeTable[cubeindex] & 2048) > 0)
+ vertlist[11] = VertexInterp(isolevel, GetPos(x, y, z, 3), GetPos(x, y, z, 7), GetVal(x, y, z, 3), GetVal(x, y, z, 7));
+
+ /* Create the triangle */
+ for (i = 0; _triTable[cubeindex][ i] != -1; i += 3)
+ {
+ _triangles[ntriang] = Triangle(vertlist[static_cast<std::size_t>(_triTable[cubeindex][i ])],
+ vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 1])],
+ vertlist[static_cast<std::size_t>(_triTable[cubeindex][i + 2])]);
+ ntriang++;
+ }
+ return ntriang;
+}
+
+Eigen::Vector3f MarchingCubes::VertexInterp(float isolevel, Eigen::Vector3f p1, Eigen::Vector3f p2, float valp1, float valp2)
+{
+ if (std::abs(isolevel - valp1) < 0.00001f)
+ return p1;
+ if (std::abs(isolevel - valp2) < 0.00001f)
+ return p2;
+ if (std::abs(valp1 - valp2) < 0.00001f)
+ return p1;
+ double mu = (isolevel - valp1) / (valp2 - valp1);
+ float x = (float)(p1.x() + mu * (p2.x() - p1.x()));
+ float y = (float)(p1.y() + mu * (p2.y() - p1.y()));
+ float z = (float)(p1.z() + mu * (p2.z() - p1.z()));
+
+ return Eigen::Vector3f(x, y, z);
+}
+
+void MarchingCubes::Build(PrimitivePtr res, int tianglesNum)
+{
+ for (int i = 0; i < tianglesNum; i++)
+ {
+ res->push_back(_triangles[i]);
+ }
+}
+
+ const int MarchingCubes::_edgeTable[256] = {
+ 0x0 , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
+ 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
+ 0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
+ 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
+ 0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,
+ 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
+ 0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,
+ 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
+ 0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,
+ 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
+ 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,
+ 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
+ 0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,
+ 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
+ 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,
+ 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
+ 0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
+ 0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
+ 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
+ 0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
+ 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
+ 0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
+ 0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
+ 0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,
+ 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
+ 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,
+ 0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
+ 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,
+ 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
+ 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,
+ 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
+ 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0
+ };
+
+ const char MarchingCubes::_triTable[256][16] =
+ {{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
+ {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
+ {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
+ {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
+ {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
+ {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
+ {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
+ {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
+ {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
+ {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
+ {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
+ {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
+ {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
+ {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
+ {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
+ {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
+ {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
+ {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
+ {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
+ {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
+ {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
+ {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
+ {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
+ {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
+ {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
+ {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
+ {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
+ {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
+ {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
+ {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
+ {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
+ {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
+ {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
+ {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
+ {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
+ {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
+ {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
+ {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
+ {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
+ {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
+ {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
+ {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
+ {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
+ {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
+ {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
+ {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
+ {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
+ {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
+ {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
+ {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
+ {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
+ {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
+ {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
+ {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
+ {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
+ {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
+ {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
+ {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
+ {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
+ {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
+ {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
+ {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
+ {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
+ {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
+ {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
+ {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
+ {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
+ {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
+ {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
+ {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
+ {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
+ {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
+ {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
+ {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
+ {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
+ {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
+ {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
+ {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
+ {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
+ {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
+ {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
+ {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
+ {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
+ {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
+ {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
+ {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
+ {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
+ {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
+ {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
+ {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
+ {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
+ {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
+ {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
+ {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
+ {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
+ {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
+ {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
+ {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
+ {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
+ {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
+ {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
+ {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
+ {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
+ {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
+ {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
+ {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
+ {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
+ {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
+ {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
+ {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
+ {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
+ {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
+ {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
+ {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
+ {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
+ {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
+ {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
+ {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
+ {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
+ {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
+ {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
+ {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
+ {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
+ {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
+ {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
+ {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
+ {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}
+};
+
+
diff --git a/VirtualRobot/math/MarchingCubes.h b/VirtualRobot/math/MarchingCubes.h
new file mode 100644
index 0000000000000000000000000000000000000000..4be723ac84cc4f036d6f8cd10175159a3e09b761
--- /dev/null
+++ b/VirtualRobot/math/MarchingCubes.h
@@ -0,0 +1,76 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_MarchingCubes
+#define math_MarchingCubes
+
+#include "MathForwardDefinitions.h"
+#include "Array3D.h"
+#include "Triangle.h"
+#include <queue>
+
+
+namespace math
+{
+
+ // code from https://nucleardevs.wordpress.com/2011/11/17/marching-cubes-sourcecode/
+ class MarchingCubes
+ {
+ public:
+ void Init(int size, Eigen::Vector3f center, Eigen::Vector3f start, int steps, float stepLength, GridCacheFloat3Ptr cache);
+ PrimitivePtr Process(float isolevel);
+ void Process(float isolevel, PrimitivePtr primitive); //TODO ref
+ PrimitivePtr ProcessSingleSurfaceOptimized(float isolevel, Index3 start);
+ void ProcessSingleSurfaceOptimized(float isolevel, PrimitivePtr primitive, Index3 start);
+ static PrimitivePtr Calculate(Eigen::Vector3f center, Eigen::Vector3f start, int steps, float stepLength, SimpleAbstractFunctionR3R1Ptr modelPtr, float isolevel);
+
+ private:
+ struct GridCell{
+ public:
+ Eigen::Vector3f P[8];
+ };
+
+ typedef boost::shared_ptr<Array3D<GridCell>> Array3DGridCellPtr;
+ typedef boost::shared_ptr<std::queue<Index3>> FringePtr;
+
+ int _size;
+ GridCacheFloat3Ptr Gdata;
+ Array3DGridCellPtr _grids;
+ Triangle _triangles[16];
+
+ static const int _edgeTable[256];
+ static const char _triTable[256][16];
+
+
+ float GetVal(int x, int y, int z, int i);
+ Eigen::Vector3f GetPos(int x, int y, int z, int i);
+ void AddAndMark(int x, int y, int z, FringePtr fringe, Array3DBoolPtr marked);
+ bool IsValidIndex(int x, int y, int z);
+
+ int Polygonise(int x, int y, int z, float isolevel);
+
+ static Eigen::Vector3f VertexInterp(float isolevel, Eigen::Vector3f p1, Eigen::Vector3f p2, float valp1, float valp2);
+
+ void Build(PrimitivePtr res, int tianglesNum);
+
+ };
+}
+
+#endif // math_MarchingCubes
diff --git a/VirtualRobot/math/MathForwardDefinitions.h b/VirtualRobot/math/MathForwardDefinitions.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f6c7b2f8b4debeb9489e3e92946aa2cf2b44653
--- /dev/null
+++ b/VirtualRobot/math/MathForwardDefinitions.h
@@ -0,0 +1,219 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_MathForwardDefinitions
+#define math_HapticExplorationLibrary_MathForwardDefinitions
+
+//scale intern sizes to milimeters
+#define HAPTIC_EXPLORATION_SCALE 40
+
+#include <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+#include <stdexcept>
+#include <vector>
+
+template<class T> class Nullable {
+public:
+ Nullable(T& value)
+ : defined(true), value(value)
+ { }
+
+ Nullable()
+ : defined(false)
+ { }
+
+ T get()
+ {
+ if(!defined)
+ {
+ throw std::exception("Value is not set");
+ }
+ return value;
+ }
+ bool hasValue()
+ {
+ return defined;
+ }
+
+private:
+ bool defined;
+ T value;
+};
+
+
+
+namespace math{
+
+
+ //typedef Eigen::Vector2f Eigen::Vector2f;
+ //typedef Eigen::Vector3f Eigen::Vector3f;
+ //typedef Eigen::Matrix3f Matrix3f;
+ //typedef Eigen::MatrixXf MatrixXf;
+ //typedef Eigen::VectorXf VectorXf;
+ //typedef Nullable<float> floatOpt;
+ typedef Nullable<Eigen::Vector3f> Vec3Opt;
+
+ class AbstractFunctionR1R2;
+ typedef boost::shared_ptr<AbstractFunctionR1R2> AbstractFunctionR1R2Ptr;
+ class Line;
+ typedef boost::shared_ptr<Line> LinePtr;
+ class LineStrip;
+ typedef boost::shared_ptr<LineStrip> LineStripPtr;
+ class AbstractFunctionR1R3;
+ typedef boost::shared_ptr<AbstractFunctionR1R3> AbstractFunctionR1R3Ptr;
+ class AbstractFunctionR2R3;
+ typedef boost::shared_ptr<AbstractFunctionR2R3> AbstractFunctionR2R3Ptr;
+ class AbstractFunctionR3R1;
+ typedef boost::shared_ptr<AbstractFunctionR3R1> AbstractFunctionR3R1Ptr;
+ class Contact;
+ typedef boost::shared_ptr<Contact> ContactPtr;
+ class ContactList;
+ typedef boost::shared_ptr<ContactList> ContactListPtr;
+ class ImplicitPlane;
+ typedef boost::shared_ptr<ImplicitPlane> ImplicitPlanePtr;
+ class LineR2;
+ typedef boost::shared_ptr<LineR2> LineR2Ptr;
+ class Plane;
+ typedef boost::shared_ptr<Plane> PlanePtr;
+ class Triangle;
+ typedef boost::shared_ptr<Triangle> TrianglePtr;
+ class SimpleAbstractFunctionR1R3;
+ typedef boost::shared_ptr<SimpleAbstractFunctionR1R3> SimpleAbstractFunctionR1R3Ptr;
+ class SimpleAbstractFunctionR2R3;
+ typedef boost::shared_ptr<SimpleAbstractFunctionR2R3> SimpleAbstractFunctionR2R3Ptr;
+ class SimpleAbstractFunctionR3R1;
+ typedef boost::shared_ptr<SimpleAbstractFunctionR3R1> SimpleAbstractFunctionR3R1Ptr;
+ class ImplicitObjectModel;
+ typedef boost::shared_ptr<ImplicitObjectModel> ImplicitObjectModelPtr;
+ class HalfSpaceObjectModel;
+ typedef boost::shared_ptr<HalfSpaceObjectModel> HalfSpaceObjectModelPtr;
+ class HalfSpaceImplicitSurface3D;
+ typedef boost::shared_ptr<HalfSpaceImplicitSurface3D> HalfSpaceImplicitSurface3DPtr;
+ class GaussianObjectModel;
+ typedef boost::shared_ptr<GaussianObjectModel> GaussianObjectModelPtr;
+ class GaussianObjectModelNormals;
+ typedef boost::shared_ptr<GaussianObjectModelNormals> GaussianObjectModelNormalsPtr;
+ class GaussianImplicitSurface3D;
+ typedef boost::shared_ptr<GaussianImplicitSurface3D> GaussianImplicitSurface3DPtr;
+ class GaussianImplicitSurface3DNormals;
+ typedef boost::shared_ptr<GaussianImplicitSurface3DNormals> GaussianImplicitSurface3DNormalsPtr;
+ class DataR3R1;
+ typedef boost::shared_ptr<DataR3R1> DataR3R1Ptr;
+ class MarchingCubes;
+ typedef boost::shared_ptr<MarchingCubes> MarchingCubesPtr;
+ class Bezier;
+ typedef boost::shared_ptr<Bezier> BezierPtr;
+ class LinearContinuedBezier;
+ typedef boost::shared_ptr<LinearContinuedBezier> LinearContinuedBezierPtr;
+ class Primitive;
+ typedef boost::shared_ptr<Primitive> PrimitivePtr;
+ class Index3;
+ typedef boost::shared_ptr<Index3> Index3Ptr;
+ class AbstractContactFeature;
+ typedef boost::shared_ptr<AbstractContactFeature> AbstractContactFeaturePtr;
+ class BinContactFeature;
+ typedef boost::shared_ptr<BinContactFeature> BinContactFeaturePtr;
+ template<class T> class Array3D;
+ typedef boost::shared_ptr<Array3D<float>> Array3DFloatPtr;
+ typedef boost::shared_ptr<Array3D<bool>> Array3DBoolPtr;
+ //typedef boost::shared_ptr<Array3D<>> Array3DPtr<T>;
+ class VoronoiWeights;
+ typedef boost::shared_ptr<VoronoiWeights> VoronoiWeightsPtr;
+ struct WeightedFloatAverage;
+ class WeightedVec3Average;
+ class Grid3D;
+ typedef boost::shared_ptr<Grid3D> Grid3DPtr;
+ class GridCacheFloat3;
+ typedef boost::shared_ptr<GridCacheFloat3> GridCacheFloat3Ptr;
+ class FeatureCluster;
+ typedef boost::shared_ptr<FeatureCluster> FeatureClusterPtr;
+ class EdgeCluster;
+ typedef boost::shared_ptr<EdgeCluster> EdgeClusterPtr;
+ class EdgePredictor;
+ typedef boost::shared_ptr<EdgePredictor> EdgePredictorPtr;
+ class EdgeTracer;
+ typedef boost::shared_ptr<EdgeTracer> EdgeTracerPtr;
+ class EdgeFeature;
+ typedef boost::shared_ptr<std::vector<Eigen::Vector3f>> Vec3ListPtr;
+ class Edge;
+ typedef boost::shared_ptr<Edge> EdgePtr;
+
+
+ typedef boost::shared_ptr<EdgeFeature> EdgeFeaturePtr;
+
+
+}
+
+namespace sim{
+ class Simulation;
+ typedef boost::shared_ptr<Simulation> SimulationPtr;
+ class HapticExplorationData;
+ typedef boost::shared_ptr<HapticExplorationData> HapticExplorationDataPtr;
+
+ namespace objects{
+
+ class AbstractObject;
+ typedef boost::shared_ptr<AbstractObject> AbstractObjectPtr;
+ class ImplicitObject;
+ typedef boost::shared_ptr<ImplicitObject> ImplicitObjectPtr;
+ class InfiniteObject;
+ typedef boost::shared_ptr<InfiniteObject> InfiniteObjectPtr;
+ class CompositeObject;
+ typedef boost::shared_ptr<CompositeObject> CompositeObjectPtr;
+ class Sphere;
+ typedef boost::shared_ptr<Sphere> SpherePtr;
+ class TriangleMeshObject;
+ typedef boost::shared_ptr<TriangleMeshObject> TriangleMeshObjectPtr;
+
+
+
+ }
+}
+
+namespace explorationControllers{
+
+ class AbstractExplorationController;
+ typedef boost::shared_ptr<AbstractExplorationController> AbstractExplorationControllerPtr;
+ class Heuristic;
+ typedef boost::shared_ptr<Heuristic> HeuristicPtr;
+ class InitialApproach;
+ typedef boost::shared_ptr<InitialApproach> InitialApproachPtr;
+ class LocalSearch;
+ typedef boost::shared_ptr<LocalSearch> LocalSearchPtr;
+ class PossibleTarget;
+ typedef boost::shared_ptr<PossibleTarget> PossibleTargetPtr;
+ class TrajectoryEdgeSearch;
+ typedef boost::shared_ptr<TrajectoryEdgeSearch> TrajectoryEdgeSearchPtr;
+ class Target;
+ typedef boost::shared_ptr<Target> TargetPtr;
+
+}
+
+
+
+
+
+
+
+
+
+
+
+#endif // math_HapticExplorationLibrary_MathForwardDefinitions
diff --git a/VirtualRobot/math/Plane.cpp b/VirtualRobot/math/Plane.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8384eae2e7a149f10332c9c4781cba49a18bbef7
--- /dev/null
+++ b/VirtualRobot/math/Plane.cpp
@@ -0,0 +1,106 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Plane.h"
+#include "Helpers.h"
+
+using namespace math;
+
+Plane::Plane(Eigen::Vector3f pos, Eigen::Vector3f dir1, Eigen::Vector3f dir2)
+ : pos(pos), dir1(dir1), dir2(dir2)
+{
+}
+
+Eigen::Vector3f Plane::GetPoint(float u, float v)
+{
+ return pos + u * dir1 + v * dir2;
+}
+
+Eigen::Vector3f Plane::GetDdu(float, float)
+{
+ return dir1;
+}
+
+Eigen::Vector3f Plane::GetDdv(float, float)
+{
+ return dir2;
+}
+
+void Plane::GetUV(Eigen::Vector3f pos, float& u, float& v)
+{
+ float w;
+ GetUVW(pos,u,v,w);
+}
+
+Eigen::Vector3f Plane::GetNormal()
+{
+ return dir1.cross(dir2);
+}
+
+Plane Plane::SwappedDirections()
+{
+ return Plane(pos, dir2, dir1);
+}
+
+Plane Plane::Normalized()
+{
+ return Plane(pos, dir1.normalized(), dir2.normalized());
+}
+
+ImplicitPlane Plane::ToImplicit()
+{
+ return ImplicitPlane::FromPositionNormal(pos, GetNormal());
+}
+
+Eigen::Matrix3f Plane::GetRotationMatrix()
+{
+ Helpers::AssertNormalized(dir1);
+ Helpers::AssertNormalized(dir2);
+ Eigen::Matrix3f result;
+ result << dir1 , dir2 , GetNormal();
+ return result;
+}
+
+std::string Plane::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << pos << ") (" << dir1 << ")" << ") (" << dir2 << ")";
+ return ss.str();
+}
+
+void Plane::GetUVW(Eigen::Vector3f pos, float& u, float& v, float& w)
+{
+ Eigen::Vector3f rotated = GetRotationMatrix().transpose() * (pos - this->pos);
+ u = rotated.x();
+ v = rotated.y();
+ w = rotated.z();
+
+}
+
+Plane Plane::FromNormal(Eigen::Vector3f pos, Eigen::Vector3f normal)
+{
+ Eigen::Vector3f d1, d2;
+ Helpers::GetOrthonormalVectors(normal, d1,d2);
+ return Plane(pos, d1, d2);
+}
+
+
+
+
diff --git a/VirtualRobot/math/Plane.h b/VirtualRobot/math/Plane.h
new file mode 100644
index 0000000000000000000000000000000000000000..65aff72771ea1d84bd31dbf8ab000662b4b7e067
--- /dev/null
+++ b/VirtualRobot/math/Plane.h
@@ -0,0 +1,73 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_Plane
+#define math_HapticExplorationLibrary_Plane
+
+#include "MathForwardDefinitions.h"
+#include "AbstractFunctionR2R3.h"
+#include "Line.h"
+#include "ImplicitPlane.h"
+
+
+
+
+
+namespace math
+{
+
+class Plane :
+ public AbstractFunctionR2R3
+{
+
+public:
+ Plane(Eigen::Vector3f pos, Eigen::Vector3f dir1, Eigen::Vector3f dir2);
+
+
+ Eigen::Vector3f Pos() {return pos;}
+ Eigen::Vector3f Dir1(){return dir1;}
+ Eigen::Vector3f Dir2(){return dir2;}
+
+
+ Eigen::Vector3f GetPoint(float u, float v) override;
+ Eigen::Vector3f GetDdu(float u, float v) override;
+ Eigen::Vector3f GetDdv(float u, float v) override;
+ void GetUV(Eigen::Vector3f pos, float &u, float &v) override;
+ Eigen::Vector3f GetNormal();
+ Plane SwappedDirections();
+ Plane Normalized();
+ ImplicitPlane ToImplicit();
+ Eigen::Matrix3f GetRotationMatrix();
+ std::string ToString();
+ void GetUVW(Eigen::Vector3f pos, float &u, float &v, float &w);
+ float GetW(Eigen::Vector3f pos);
+ float Intersect(Line line, bool* exists);
+ Line Intersect(Plane p2);
+
+ static Plane FromNormal(Eigen::Vector3f pos, Eigen::Vector3f normal);
+private:
+ Eigen::Vector3f pos;
+ Eigen::Vector3f dir1;
+ Eigen::Vector3f dir2;
+
+};
+}
+
+#endif // math_HapticExplorationLibrary_Plane
diff --git a/VirtualRobot/math/Primitive.cpp b/VirtualRobot/math/Primitive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d68c15ea1c5ee8addded0319a58fd819326eb99
--- /dev/null
+++ b/VirtualRobot/math/Primitive.cpp
@@ -0,0 +1,37 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Line.h"
+#include "Primitive.h"
+
+using namespace math;
+
+
+
+void Primitive::AddTriangle(Eigen::Vector3f v1, Eigen::Vector3f v2, Eigen::Vector3f v3)
+{
+ push_back(Triangle(v1, v2, v3));
+}
+
+
+
+
+
+
diff --git a/VirtualRobot/math/Primitive.h b/VirtualRobot/math/Primitive.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d85295aa6352efbe38fc8e7f1db2056ce21bbc9
--- /dev/null
+++ b/VirtualRobot/math/Primitive.h
@@ -0,0 +1,40 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Primitives
+#define math_Primitives
+
+#include "MathForwardDefinitions.h"
+#include "Triangle.h"
+
+namespace math
+{
+
+ class Primitive :
+ public std::vector<Triangle>
+ {
+ public:
+ void AddTriangle(Eigen::Vector3f v1, Eigen::Vector3f v2, Eigen::Vector3f v3);
+ private:
+ };
+
+}
+
+#endif // math_Primitive
diff --git a/VirtualRobot/math/SimpleAbstractFunctionR1R3.h b/VirtualRobot/math/SimpleAbstractFunctionR1R3.h
new file mode 100644
index 0000000000000000000000000000000000000000..cb291b54ff360bbeb67d44de624d074fa16092bc
--- /dev/null
+++ b/VirtualRobot/math/SimpleAbstractFunctionR1R3.h
@@ -0,0 +1,40 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_SimpleAbstractFunctionR1R3
+#define math_HapticExplorationLibrary_SimpleAbstractFunctionR1R3
+
+#include "MathForwardDefinitions.h"
+
+
+
+namespace math
+{
+
+ class SimpleAbstractFunctionR1R3
+ {
+ public:
+ virtual Eigen::Vector3f Get(float t) = 0;
+
+ private:
+ };
+}
+
+#endif // math_HapticExplorationLibrary_SimpleAbstractFunctionR1R3
diff --git a/VirtualRobot/math/SimpleAbstractFunctionR2R3.h b/VirtualRobot/math/SimpleAbstractFunctionR2R3.h
new file mode 100644
index 0000000000000000000000000000000000000000..85ba6f39a648a9467c73ed51891a32a53867a0eb
--- /dev/null
+++ b/VirtualRobot/math/SimpleAbstractFunctionR2R3.h
@@ -0,0 +1,41 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_SimpleAbstractFunctionR2R3
+#define math_HapticExplorationLibrary_SimpleAbstractFunctionR2R3
+
+#include "MathForwardDefinitions.h"
+
+
+
+namespace math
+{
+
+class SimpleAbstractFunctionR2R3
+{
+public:
+ virtual Eigen::Vector3f GetPoint(float u, float v) = 0;
+ Eigen::Vector3f GetPoint(Eigen::Vector3f uv){return GetPoint(uv.x(),uv.y());}
+
+private:
+};
+}
+
+#endif // math_HapticExplorationLibrary_SimpleAbstractFunctionR2R3
diff --git a/VirtualRobot/math/SimpleAbstractFunctionR3R1.h b/VirtualRobot/math/SimpleAbstractFunctionR3R1.h
new file mode 100644
index 0000000000000000000000000000000000000000..09d0d3389b093dc0164272c573d9f5507d556850
--- /dev/null
+++ b/VirtualRobot/math/SimpleAbstractFunctionR3R1.h
@@ -0,0 +1,40 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_HapticExplorationLibrary_SimpleAbstractFunctionR3R1
+#define math_HapticExplorationLibrary_SimpleAbstractFunctionR3R1
+
+#include "MathForwardDefinitions.h"
+
+
+
+namespace math
+{
+
+ class SimpleAbstractFunctionR3R1
+ {
+ public:
+ virtual float Get(Eigen::Vector3f pos) = 0;
+
+ private:
+ };
+}
+
+#endif // math_HapticExplorationLibrary_SimpleAbstractFunctionR3R1
diff --git a/VirtualRobot/math/Triangle.cpp b/VirtualRobot/math/Triangle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06cd62cd76eed8de75cccb1f5c33377a067964f5
--- /dev/null
+++ b/VirtualRobot/math/Triangle.cpp
@@ -0,0 +1,71 @@
+#include "Triangle.h"
+
+using namespace math;
+
+
+Triangle::Triangle()
+ : p1(Eigen::Vector3f(0,0,0)), p2(Eigen::Vector3f(0,0,0)), p3(Eigen::Vector3f(0,0,0))
+{
+}
+
+Triangle::Triangle(Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3)
+ : p1(p1), p2(p2), p3(p3)
+{
+}
+
+std::string Triangle::ToString()
+{
+ std::stringstream ss;
+ ss << "(" << p1 << ") (" << p2 << ") (" << p3 << ")";
+ return ss.str();
+}
+
+Eigen::Vector3f Triangle::Normal() const
+{
+ return (p2 - p1).cross(p3 - p1);
+}
+
+Triangle Triangle::Flipped()
+{
+ return Triangle(p2, p1, p3);
+}
+
+Eigen::Vector3f Triangle::Centroid()
+{
+ return (p1 + p2 + p3) / 3;
+}
+
+std::vector<Triangle> Triangle::Subdivide(int depth)
+{
+ std::vector<Triangle> list;
+ Subdivide(depth, p1, p2, p3, list);
+ return list;
+}
+
+void Triangle::Subdivide(int depth, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, std::vector<Triangle> &list)
+{
+ if (depth <= 0)
+ {
+ list.push_back(Triangle(p1, p2, p3));
+ return;
+ }
+ Eigen::Vector3f c = (p1 + p2 + p3) / 3;
+ Subdivide(depth - 1, p1, p2, c, list);
+ Subdivide(depth - 1, p2, p3, c, list);
+ Subdivide(depth - 1, p3, p1, c, list);
+}
+
+Triangle Triangle::Transform(Eigen::Vector3f center, float unitLength)
+{
+ return Triangle(
+ p1 * unitLength + center,
+ p2 * unitLength + center,
+ p3 * unitLength + center);
+}
+
+
+
+
+
+
+
diff --git a/VirtualRobot/math/Triangle.h b/VirtualRobot/math/Triangle.h
new file mode 100644
index 0000000000000000000000000000000000000000..72cab37a83929de97758fab975e6bc19a76bde9f
--- /dev/null
+++ b/VirtualRobot/math/Triangle.h
@@ -0,0 +1,59 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Martin Miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_Triangle
+#define math_Triangle
+
+#include "MathForwardDefinitions.h"
+
+
+namespace math
+{
+
+ class Triangle
+ {
+ public:
+ Triangle();//TODO
+ Triangle(Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3);
+
+ Eigen::Vector3f P1() const {return p1;}
+ Eigen::Vector3f P2() const {return p2;}
+ Eigen::Vector3f P3() const {return p3;}
+
+ std::string ToString();
+
+ Eigen::Vector3f Normal() const;
+
+ Triangle Flipped();
+
+ Eigen::Vector3f Centroid();
+ std::vector<Triangle> Subdivide(int depth);
+
+ static void Subdivide(int depth, Eigen::Vector3f p1, Eigen::Vector3f p2, Eigen::Vector3f p3, std::vector<Triangle> &list);
+
+ Triangle Transform(Eigen::Vector3f center, float unitLength);
+ private:
+ Eigen::Vector3f p1;
+ Eigen::Vector3f p2;
+ Eigen::Vector3f p3;
+ };
+}
+
+#endif //math_Triangle
diff --git a/VirtualRobot/math/WeightedAverage.cpp b/VirtualRobot/math/WeightedAverage.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ab51c86b263e080ddf880aa4092ce18fba32061f
--- /dev/null
+++ b/VirtualRobot/math/WeightedAverage.cpp
@@ -0,0 +1,56 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "WeightedAverage.h"
+
+using namespace math;
+
+
+
+void math::WeightedFloatAverage::Add(float value, float weight)
+{
+ sum += value * weight;
+ weightSum += weight;
+}
+
+float WeightedFloatAverage::Average()
+{
+ return sum / weightSum;
+}
+
+float WeightedFloatAverage::WeightSum()
+{
+ return weightSum;
+}
+void math::WeightedVec3Average::Add(Eigen::Vector3f value, float weight)
+{
+ sum += value * weight;
+ weightSum += weight;
+}
+
+Eigen::Vector3f WeightedVec3Average::Average()
+{
+ return sum / weightSum;
+}
+
+float WeightedVec3Average::WeightSum()
+{
+ return weightSum;
+}
diff --git a/VirtualRobot/math/WeightedAverage.h b/VirtualRobot/math/WeightedAverage.h
new file mode 100644
index 0000000000000000000000000000000000000000..58c5b1046518da31f889c9e80c61fccff6778c95
--- /dev/null
+++ b/VirtualRobot/math/WeightedAverage.h
@@ -0,0 +1,53 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author miller (martin dot miller at student dot kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#ifndef math_WeightedAverage
+#define math_WeightedAverage
+
+
+#include "MathForwardDefinitions.h"
+
+namespace math
+{
+
+ struct WeightedFloatAverage
+ {
+ public:
+ void Add(float value, float weight);
+ float Average();
+ float WeightSum();
+ private:
+ float sum = 0;
+ float weightSum = 0;
+ };
+
+ class WeightedVec3Average
+ {
+ public:
+ void Add(Eigen::Vector3f value, float weight);
+ Eigen::Vector3f Average();
+ float WeightSum();
+ private:
+ Eigen::Vector3f sum;
+ float weightSum = 0;
+ };
+}
+
+#endif // math_WeightedAverage
diff --git a/VirtualRobot/tests/CMakeLists.txt b/VirtualRobot/tests/CMakeLists.txt
index 336a14e33d21a4261decfa678a02ce54a826e3f2..1d48573045a8226cb8c4492410a40c49411467c4 100644
--- a/VirtualRobot/tests/CMakeLists.txt
+++ b/VirtualRobot/tests/CMakeLists.txt
@@ -29,3 +29,5 @@ ADD_VR_TEST( VirtualRobotGazeIKTest )
ADD_VR_TEST( VirtualRobotMeshImportTest )
ADD_VR_TEST( VirtualRobotTimeOptimalTrajectoryTest )
+
+ADD_VR_TEST( MathGaussianImplicitSurface3DNormalsTest )
diff --git a/VirtualRobot/tests/MathGaussianImplicitSurface3DNormals.cpp b/VirtualRobot/tests/MathGaussianImplicitSurface3DNormals.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55db998f0218b69fa8bf24044ad0c83911216bb7
--- /dev/null
+++ b/VirtualRobot/tests/MathGaussianImplicitSurface3DNormals.cpp
@@ -0,0 +1,352 @@
+/**
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_VirtualRobotWorkSpaceTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Workspace/WorkspaceRepresentation.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Workspace/Reachability.h>
+#include <string>
+
+BOOST_AUTO_TEST_SUITE(WorkSpace)
+
+BOOST_AUTO_TEST_CASE(testWorkSpaceEuler)
+{
+ const std::string robotString =
+ "<Robot Type='MyDemoRobotType' StandardName='ExampleRobo' RootNode='Joint1'>"
+ " <RobotNode name='Joint1'>"
+ " <Visualization enable='true'>"
+ " </Visualization>"
+ " </RobotNode>"
+ "</Robot>";
+ VirtualRobot::RobotPtr rob;
+ BOOST_REQUIRE_NO_THROW(rob = VirtualRobot::RobotIO::createRobotFromString(robotString));
+ BOOST_REQUIRE(rob);
+ VirtualRobot::WorkspaceRepresentationPtr ws;
+ BOOST_REQUIRE_NO_THROW(ws.reset(new VirtualRobot::WorkspaceRepresentation(rob)));
+ ws->setOrientationType(VirtualRobot::WorkspaceRepresentation::Hopf);
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ float x[6];
+ Eigen::Vector3f ax;
+ float a;
+ Eigen::Matrix3f m3;
+
+ // identity, matrix -> vector
+ ws->matrix2Vector(m, x);
+
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(x[3], 1e-6f);
+ BOOST_CHECK_SMALL(x[4], 1e-6f);
+ BOOST_CHECK_SMALL(x[5], 1e-6f);
+
+ // identity, vector -> matrix
+ for (int i = 0; i < 6; i++)
+ {
+ x[i] = 0.0f;
+ }
+
+ ws->vector2Matrix(x, m);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+
+ VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
+ BOOST_CHECK_CLOSE(a, 0.0f, 1e-6f);
+
+ // rot x
+
+ m.setIdentity();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitX()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+
+
+ ws->matrix2Vector(m, x);
+ ws->vector2Matrix(x, m);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+ VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(0) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(1), 1e-6f);
+ BOOST_CHECK_SMALL(ax(2), 1e-6f);
+
+
+ // rot y
+ m.setIdentity();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitY()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ ws->matrix2Vector(m, x);
+ ws->vector2Matrix(x, m);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+ VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(1) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(0), 1e-6f);
+ BOOST_CHECK_SMALL(ax(2), 1e-6f);
+
+ // rot z
+ m.setIdentity();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitZ()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ ws->matrix2Vector(m, x);
+ ws->vector2Matrix(x, m);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+ VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(2) - 1.0f, 1e-6f);
+ BOOST_CHECK_SMALL(ax(1), 1e-6f);
+ BOOST_CHECK_SMALL(ax(0), 1e-6f);
+
+
+ // rot x,y
+ m.setIdentity();
+ ax << 1.0f, 1.0f, 0.0f;
+ ax.normalize();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, ax).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ ws->matrix2Vector(m, x);
+ ws->vector2Matrix(x, m);
+ BOOST_CHECK_SMALL(x[0], 1e-6f);
+ BOOST_CHECK_SMALL(x[1], 1e-6f);
+ BOOST_CHECK_SMALL(x[2], 1e-6f);
+ BOOST_CHECK_SMALL(m(0, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(1, 3), 1e-6f);
+ BOOST_CHECK_SMALL(m(2, 3), 1e-6f);
+ VirtualRobot::MathTools::eigen4f2axisangle(m, ax, a);
+ BOOST_CHECK_SMALL(a - float(M_PI) / 4.0f, 1e-3f);
+ BOOST_CHECK_SMALL(ax(0) - 1.0f / float(sqrt(2.0f)), 1e-3f);
+ BOOST_CHECK_SMALL(ax(1) - 1.0f / float(sqrt(2.0f)), 1e-3f);
+ BOOST_CHECK_SMALL(ax(2), 1e-4f);
+
+}
+
+BOOST_AUTO_TEST_CASE(testWorkSpaceNeighbors)
+{
+ // CREATE ROBOT AND DATA STRUCTURES
+ const std::string robotString =
+ "<Robot Type='MyDemoRobotType' StandardName='ExampleRobo' RootNode='root'>"
+ " <RobotNode name='root'>"
+ " <Child name='joint1'/>"
+ " </RobotNode>"
+ " <RobotNode name='joint1'>"
+ " <Joint type='revolute'>"
+ " <Limits unit='degree' lo='0' hi='90'/>"
+ " <axis x='0' y='0' z='1'/>"
+ " </Joint>"
+ " <Child name='tcp'/>"
+ " </RobotNode>"
+ " <RobotNode name='tcp'>"
+ " <Transform>"
+ " <Translation x='100' y='0' z='0'/>"
+ " </Transform>"
+ " </RobotNode>"
+ "</Robot>";
+ VirtualRobot::RobotPtr rob;
+ BOOST_REQUIRE_NO_THROW(rob = VirtualRobot::RobotIO::createRobotFromString(robotString));
+ BOOST_REQUIRE(rob);
+
+ std::vector<std::string> rnsNames;
+ rnsNames.push_back("joint1");
+ VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(rob, "rns", rnsNames, "", "tcp", true);
+ BOOST_REQUIRE(rns);
+ BOOST_REQUIRE(rob->hasRobotNodeSet("rns"));
+
+ VirtualRobot::RobotNodePtr joint1 = rob->getRobotNode("joint1");
+ BOOST_REQUIRE(joint1);
+ VirtualRobot::RobotNodePtr tcp = rob->getRobotNode("tcp");
+ BOOST_REQUIRE(tcp);
+ VirtualRobot::RobotNodePtr rootNode = rob->getRobotNode("root");
+ BOOST_REQUIRE(rootNode);
+
+ // CHECK ROBOT WORKSPACE
+ Eigen::Matrix4f tcpPose;
+ Eigen::Matrix4f gp;
+
+ // reset rob
+ gp = Eigen::Matrix4f::Identity();
+ rob->setGlobalPose(gp);
+ joint1->setJointValue(0);
+
+
+ // CREATE REACHABILITY DATA
+ static float discrTr = 10.0f;
+ static float discrRot = 0.5f;
+ static float discrTr3 = discrTr*sqrt(3);
+ static float discrRot3 = discrRot*sqrt(3);
+ float minBounds[6] = {0,0,0,0,0,0};
+ float maxBounds[6] = {100.0f,100.0f,100.0f,2*M_PI,2*M_PI,2*M_PI};
+ VirtualRobot::ReachabilityPtr reach(new VirtualRobot::Reachability(rob));
+ BOOST_REQUIRE(reach);
+ reach->setOrientationType(VirtualRobot::WorkspaceRepresentation::Hopf);
+ reach->initialize(rns, discrTr, discrRot, minBounds, maxBounds, VirtualRobot::SceneObjectSetPtr(), VirtualRobot::SceneObjectSetPtr(), rootNode, tcp);
+/*
+ float jv = M_PI/4;
+ joint1->setJointValue(jv);
+ reach->addCurrentTCPPose();
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ float x[6];*/
+
+ Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f m2 = Eigen::Matrix4f::Identity();
+ float x[6];
+ unsigned int v[6];
+ float diffRot,diffPos;
+ bool poseOK;
+
+ // identity, matrix -> voxel -> matrix
+ poseOK = reach->getVoxelFromPose(m,v);
+ BOOST_REQUIRE(poseOK);
+ m2 = reach->getPoseFromVoxel(v);
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+ // rot x, matrix -> voxel -> matrix
+ m.setIdentity();
+ Eigen::Matrix3f m3 = Eigen::AngleAxisf(float(M_PI) / 4.0f, Eigen::Vector3f::UnitX()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ poseOK = reach->getVoxelFromPose(m,v);
+ BOOST_REQUIRE(poseOK);
+ m2 = reach->getPoseFromVoxel(v);
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+ // rot y, matrix -> voxel -> matrix
+ m.setIdentity();
+ m3 = Eigen::AngleAxisf(-float(M_PI) / 4.0f, Eigen::Vector3f::UnitY()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ poseOK = reach->getVoxelFromPose(m,v);
+ BOOST_REQUIRE(poseOK);
+ m2 = reach->getPoseFromVoxel(v);
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+ // rot z, matrix -> voxel -> matrix
+ m.setIdentity();
+ m3 = Eigen::AngleAxisf(float(M_PI) / 2.0f, Eigen::Vector3f::UnitZ()).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ poseOK = reach->getVoxelFromPose(m,v);
+ BOOST_REQUIRE(poseOK);
+ m2 = reach->getPoseFromVoxel(v);
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+ const int NR_TESTS = 1000;
+ for (int i=0;i<NR_TESTS;i++)
+ {
+ Eigen::Vector3f ax = Eigen::Vector3f::Random();
+ ax.normalize();
+ float ang = rand() % 1000 / 1000.0f * 2.0f*M_PI -M_PI;
+ float xa = rand() % 1000 / 1000.0f * 100.0f;
+ float ya = rand() % 1000 / 1000.0f * 100.0f;
+ float za = rand() % 1000 / 1000.0f * 100.0f;
+ m3 = Eigen::AngleAxisf(ang, ax).matrix();
+ m.block(0, 0, 3, 3) = m3;
+ m(0,3) = xa;
+ m(1,3) = ya;
+ m(2,3) = za;
+
+ reach->matrix2Vector(m, x);
+ reach->vector2Matrix(x, m2);
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ if (diffPos > discrTr || diffRot > discrRot)
+ {
+ std::cout << "matrix<->vect: Pose diff: tr:" << diffPos << ", rot:" << diffRot << ", p:" << xa << "," << ya << "," << za << ", ax:" << ax.transpose() << ", ang:" << ang << std::endl;
+ }
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+
+ poseOK = reach->getVoxelFromPose(m,v);
+ BOOST_REQUIRE(poseOK);
+ if (!poseOK)
+ {
+ std::cout << "pose!ok:" << m << std::endl;
+ //reach->getVoxelFromPose(m,v);
+ continue;
+ }
+ m2 = reach->getPoseFromVoxel(v);
+ /*float x2[6];
+ float x3[6];
+ float v2[6];
+ float v3[6];
+ for (int i=0;i<6;i++)
+ v2[i] = (float)v[i];
+ for (int i=0;i<6;i++)
+ v3[i] = (float)v[i] + 0.5f;
+ reach->getPoseFromVoxelLocal(v2,x2);
+ reach->getPoseFromVoxelLocal(v3,x3);*/
+
+ VirtualRobot::MathTools::getPoseDiff(m,m2,diffPos,diffRot);
+ if (diffPos > discrTr3 || diffRot > discrRot3)
+ {
+ std::cout << "Pose diff: tr:" << diffPos << ", rot:" << diffRot << ", p:" << xa << "," << ya << "," << za << ", ax:" << ax.transpose() << ", ang:" << ang << std::endl;
+ /*std::cout << "vOrig:";
+ for (int i=0;i<6;i++)
+ std::cout << v[i] << ",";
+ std::cout << std::endl;
+ reach->getVoxelFromPose(m2,v);
+ std::cout << "v2New:";
+ for (int i=0;i<6;i++)
+ std::cout << v[i] << ",";
+ std::cout << std::endl;
+ std::cout << "xOrig:";
+ for (int i=0;i<6;i++)
+ std::cout << x[i] << ",";
+ std::cout << std::endl;
+ std::cout << "x2New:";
+ for (int i=0;i<6;i++)
+ std::cout << x2[i] << ",";
+ std::cout << std::endl;
+ std::cout << "x3+0.5:";
+ for (int i=0;i<6;i++)
+ std::cout << x3[i] << ",";
+ std::cout << std::endl;
+
+ float xT1[6];
+ xT1[0] = x[0];
+ xT1[1] = x[1];
+ xT1[2] = x[2];
+ xT1[3] = x2[3];
+ xT1[4] = x2[4];
+ xT1[5] = x2[5];
+ Eigen::Matrix4f mT1;
+ reach->vector2Matrix(xT1,mT1);
+ VirtualRobot::MathTools::getPoseDiff(m,mT1,diffPos,diffRot);
+ std::cout << "T1 Pose diff: tr:" << diffPos << ", rot:" << diffRot << std::endl;*/
+ }
+ BOOST_REQUIRE_LE(diffPos, discrTr3);
+ BOOST_REQUIRE_LE(diffRot, discrRot3);
+
+ }
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/VirtualRobot/tests/MathGaussianImplicitSurface3DNormalsTest.cpp b/VirtualRobot/tests/MathGaussianImplicitSurface3DNormalsTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb527bec007e365f84e9fd032c1ed7f40a8fc3e6
--- /dev/null
+++ b/VirtualRobot/tests/MathGaussianImplicitSurface3DNormalsTest.cpp
@@ -0,0 +1,34 @@
+/**
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2012 Nikolaus Vahrenkamp
+*/
+
+#define BOOST_TEST_MODULE VirtualRobot_MathGaussianImplicitSurface3DNormalsTest
+
+#include <VirtualRobot/VirtualRobotTest.h>
+#include <VirtualRobot/math/GaussianImplicitSurface3DNormals.h>
+#include <string>
+#include <stdio.h>
+
+BOOST_AUTO_TEST_SUITE(MathGaussianImplicitSurface3DNormals)
+
+using namespace math;
+
+typedef Eigen::Vector3f Vec3;
+
+BOOST_AUTO_TEST_CASE(testGPIS)
+{
+ ContactList contacts;
+ contacts.push_back(Contact(1,0,0,1,0,0));
+ contacts.push_back(Contact(0,1,0,0,1,0));
+ GaussianImplicitSurface3DNormals gpis;
+ gpis.Calculate(contacts, 0, 0, 1);
+ BOOST_CHECK_LE(gpis.Get(Vec3(0,0,0)), -0.2);
+ BOOST_CHECK_LE(abs(gpis.Get(Vec3(1,0,0))), 0.001);
+ BOOST_CHECK_LE(abs(gpis.Get(Vec3(0,1,0))), 0.001);
+ BOOST_CHECK_GE(abs(gpis.Get(Vec3(2,0,0))), 1.5);
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()