added CreateFrom... in Grid3D, made methods const

VirtualRobot/math/Grid3D.cpp

@@ -27,28 +27,38 @@ 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;
-}
+    : p1(p1), p2(p2), stepsX(stepsX), stepsY(stepsY), 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())));
+    return Grid3DPtr(new Grid3D(p1, p2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
+}
+
+Grid3DPtr Grid3D::CreateFromCenterAndSize(const Eigen::Vector3f &center, const Eigen::Vector3f &size, float stepLength)
+{
+    return CreateFromBox(center - size / 2, center + size / 2, stepLength);
+}
+
+Grid3DPtr Grid3D::CreateFromCenterAndSteps(const Eigen::Vector3f &center, const Eigen::Vector3f &steps, float stepLength)
+{
+    return Grid3DPtr(new Grid3D(center - steps * stepLength / 2, center + steps * stepLength / 2, std::round(steps.x()), std::round(steps.y()), std::round(steps.z())));
 }
 
 
-Eigen::Vector3f Grid3D::Get(int x, int y, int z){
+Eigen::Vector3f Grid3D::Get(int x, int y, int z) const{
     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));
+                           Helpers::Lerp(p1.y(), p2.y(), 0, stepsY, y),
+                           Helpers::Lerp(p1.z(), p2.z(), 0, stepsZ, z));
+}
+
+Eigen::Vector3f Grid3D::Get(const Eigen::Vector3i &index) const
+{
+    return Get(index.x(), index.y(), index.z());
 }
 
-std::vector<Eigen::Vector3f> Grid3D::AllGridPoints()
+std::vector<Eigen::Vector3f> Grid3D::AllGridPoints() const
 {
     std::vector<Eigen::Vector3f> points;
     for (int x = 0; x <= stepsX; x++)
@@ -63,3 +73,30 @@ std::vector<Eigen::Vector3f> Grid3D::AllGridPoints()
     }
     return points;
 }
+
+Eigen::Vector3i Grid3D::GetFirstIndex() const
+{
+    return Eigen::Vector3i::Zero();
+}
+
+bool Grid3D::IncrementIndex(Eigen::Vector3i &index) const
+{
+    Eigen::Vector3i steps = Steps();
+    for(int i = 0; i < 3; i++)
+    {
+        index(i)++;
+        if(index(i) <= steps(i)) return true;
+        index(i) = 0;
+    }
+    return false;
+}
+
+bool Grid3D::IndexValid(const Eigen::Vector3i &index) const
+{
+    Eigen::Vector3i steps = Steps();
+    for(int i = 0; i < 3; i++)
+    {
+        if(index(i) < 0 || index(i) > steps(i)) return false;
+    }
+    return true;
+}

VirtualRobot/math/Grid3D.h

@@ -28,24 +28,31 @@ 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;  }
+         Eigen::Vector3f P1() const { return p1;  }
+         Eigen::Vector3f P2() const { return p2;  }
+         int StepsX() const { return stepsX; }
+         int StepsY() const { return stepsY; }
+         int StepsZ() const { return stepsZ; }
+         Eigen::Vector3i Steps() const { return Eigen::Vector3i(stepsX, stepsY, 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);
+        static Grid3DPtr CreateFromCenterAndSize(const Eigen::Vector3f& center, const Eigen::Vector3f& size, float stepLength);
+        static Grid3DPtr CreateFromCenterAndSteps(const Eigen::Vector3f& center, const Eigen::Vector3f &steps, float stepLength);
 
-        Eigen::Vector3f Get(int x, int y, int z);
-        std::vector<Eigen::Vector3f> AllGridPoints();
+        Eigen::Vector3f Get(int x, int y, int z) const;
+        Eigen::Vector3f Get(const Eigen::Vector3i& index) const;
+        std::vector<Eigen::Vector3f> AllGridPoints() const;
+        Eigen::Vector3i GetFirstIndex() const;
+        bool IncrementIndex(Eigen::Vector3i& index) const;
+        bool IndexValid(const Eigen::Vector3i& index) const;
 
     private :
-        Eigen::Vector3f p1;
-        Eigen::Vector3f p2;
-        int stepsX;
-        int stepsY;
-        int stepsZ;
+        const Eigen::Vector3f p1;
+        const Eigen::Vector3f p2;
+        const int stepsX;
+        const int stepsY;
+        const int stepsZ;
 
     };
 }