ENH: Refactor the getVelocity method

in the Scene objects.

SceneModels/DeformableSceneObject.h

@@ -86,6 +86,15 @@ public:
     ///
     std::shared_ptr<OdeSystemState> getPreviousState();
 
+    ///
+    /// \brief Returns velocity of at a given location for the current state.
+    ///
+    Eigen::Map<core::Vec3d> getVelocity(const int index)
+    {
+        auto velocities = this->currentState->getVelocities();
+        return Eigen::Map<core::Vec3d>(&velocities(index));
+    }
+
 private:
     ///////////////////////////////////////////////////////////////////////////////
     //////////// TODO: These are pure virtual methods from superclass. ////////////

SceneModels/SceneObject.cpp

@@ -143,9 +143,10 @@ const std::unordered_map< int,core::Vec3d> &SceneObject::getContactPoints() cons
 }
 
 //---------------------------------------------------------------------------
-core::Vec3d SceneObject::getVelocity(const int) const
+Eigen::Map<core::Vec3d> SceneObject::getVelocity(const int)
 {
-    return core::Vec3d::Zero();
+    // TODO: Dangerous - Make this function pure virtual.
+    return Eigen::Map<core::Vec3d>(nullptr);
 }
 
 //---------------------------------------------------------------------------

SceneModels/SceneObject.h

@@ -188,7 +188,7 @@ public:
     /// \brief Returns velocity of at a given location
     ///     (not given node) in contact force vector
     ///
-    virtual core::Vec3d getVelocity(const int) const;
+    virtual Eigen::Map<core::Vec3d> getVelocity(const int);
 
     ///
     /// \brief Set all contact forces to zero (if any)

Simulators/SceneObjectDeformable.cpp

@@ -54,12 +54,10 @@ void SceneObjectDeformable::applyContactForces()
     }
 }
 
-
-core::Vec3d SceneObjectDeformable::getVelocity(const int dofID) const
+Eigen::Map<core::Vec3d> SceneObjectDeformable::getVelocity(const int dofID)
 {
-    core::Vec3d vel(uvel[dofID], uvel[dofID + 1], uvel[dofID + 2]);
-
-    return vel;
+    double *pointer = &uvel[dofID];
+    return Eigen::Map<core::Vec3d>(pointer);
 }
 
 core::Vec3d SceneObjectDeformable::getDisplacementOfNodeWithDofID(const int dofID) const

Simulators/SceneObjectDeformable.h

@@ -75,7 +75,7 @@ public:
 
     /// \brief  returns velocity of at a given location
     /// (not given node) in contact force vector
-    core::Vec3d getVelocity(const int dofID) const;
+    Eigen::Map<core::Vec3d> getVelocity(const int dofID);
 
     /// \brief  returns acceleration of at a given location
     /// (not given node) in contact force vector