ENH: Add translation & orientation API's

Base/Core/imstkMath.h

@@ -43,12 +43,17 @@ using VecNd = Eigen::VectorXd;
 using Quatf = Eigen::Quaternionf;
 using Quatd = Eigen::Quaterniond;
 
+// 3x3 Matrix
+using Mat3f = Eigen::Matrix<float, 3, 3>;
+using Mat3d = Eigen::Matrix<double, 3, 3>;
+
 #define UP Vec3d(0.0, 1.0, 0.0)
 #define DOWN Vec3d(0, -1, 0)
 #define RIGHT Vec3d(1, 0, 0)
 #define LEFT Vec3d(-1, 0, 0)
 #define FORWARD Vec3d(0, 0, 1)
 #define BACKWARD Vec3d(0, 0, -1)
+#define ORIGIN Vec3d(0, 0, 0)
 }
 
 #endif // ifndef imstkMath_h

Base/Geometry/imstkAnalyticalGeometry.cpp

@@ -28,12 +28,26 @@ AnalyticalGeometry::translate(const Vec3d& t)
     m_position += t;
 }
 
+void
+AnalyticalGeometry::translate(const double& x,
+                              const double& y,
+                              const double& z)
+{
+    this->translate(Vec3d(x, y, z));
+}
+
 void
 AnalyticalGeometry::rotate(const Quatd& r)
 {
     m_orientation = r * m_orientation;
 }
 
+void
+AnalyticalGeometry::rotate(const Mat3d& r)
+{
+    this->rotate(Quatd(r));
+}
+
 void
 AnalyticalGeometry::rotate(const Vec3d& axis, const double& angle)
 {
@@ -53,9 +67,11 @@ AnalyticalGeometry::setPosition(const Vec3d& position)
 }
 
 void
-AnalyticalGeometry::setOrientation(const Quatd& orientation)
+AnalyticalGeometry::setPosition(const double& x,
+                                const double& y,
+                                const double& z)
 {
-    m_orientation = orientation;
+    this->setPosition(Vec3d(x, y, z));
 }
 
 const Quatd&
@@ -63,4 +79,22 @@ AnalyticalGeometry::getOrientation() const
 {
     return m_orientation;
 }
+
+void
+AnalyticalGeometry::setOrientation(const Quatd& orientation)
+{
+    m_orientation = orientation;
+}
+
+void
+AnalyticalGeometry::setOrientation(const Mat3d& orientation)
+{
+    this->setOrientation(Quatd(orientation));
+}
+
+void
+AnalyticalGeometry::setOrientation(const Vec3d& axis, const double& angle)
+{
+    this->setOrientation(Quatd(Eigen::AngleAxisd(angle, axis)));
+}
 }

Base/Geometry/imstkAnalyticalGeometry.h

@@ -32,15 +32,25 @@ public:
     ~AnalyticalGeometry() = default;
 
     void         translate(const Vec3d& t) override;
+    void         translate(const double& x,
+                           const double& y,
+                           const double& z) override;
     void         rotate(const Quatd& r) override;
+    void         rotate(const Mat3d& r) override;
     void         rotate(const Vec3d & axis,
                         const double& angle) override;
 
     const Vec3d& getPosition() const;
     void         setPosition(const Vec3d& position);
+    void         setPosition(const double& x,
+                             const double& y,
+                             const double& z);
 
     const Quatd& getOrientation() const;
     void         setOrientation(const Quatd& orientation);
+    void         setOrientation(const Mat3d& orientation);
+    void         setOrientation(const Vec3d & axis,
+                                const double& angle);
 
 protected:
 

Base/Geometry/imstkGeometry.h

@@ -42,9 +42,13 @@ public:
     ~Geometry() = default;
 
     virtual void        translate(const Vec3d& t) = 0;
-    virtual void        rotate(const Quatd& r)    = 0;
+    virtual void        translate(const double& x,
+                                  const double& y,
+                                  const double& z) = 0;
+    virtual void        rotate(const Quatd& r)     = 0;
     virtual void        rotate(const Vec3d & axis,
                                const double& angle) = 0;
+    virtual void        rotate(const Mat3d& r)      = 0;
 
     const GeometryType& getType() const;
 

Base/Geometry/imstkPlane.cpp

@@ -23,7 +23,7 @@
 
 namespace imstk {
 Vec3d
-Plane::getNormal()
+Plane::getNormal() const
 {
     return m_orientation._transformVector(UP);
 }

Base/Geometry/imstkPlane.h

@@ -29,7 +29,7 @@ class Plane : public AnalyticalGeometry
 {
 public:
 
-    Plane(const Vec3d & position = Vec3d(),
+    Plane(const Vec3d & position = ORIGIN,
           const Vec3d & normal = UP,
           const double& width = 100) :
         AnalyticalGeometry(GeometryType::Plane,
@@ -40,7 +40,7 @@ public:
 
     ~Plane() = default;
 
-    Vec3d         getNormal();
+    Vec3d         getNormal() const;
     void          setNormal(const Vec3d& normal);
 
     const double& getWidth() const;