Merge pull request #561 from dartsim/vsk_parser

VSK file parser

dart/dynamics/BoxShape.cpp

@@ -35,8 +35,8 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <cmath>
 #include "dart/dynamics/BoxShape.h"
-
 #include "dart/renderer/RenderInterface.h"
 
 namespace dart {
@@ -50,21 +50,39 @@ BoxShape::BoxShape(const Eigen::Vector3d& _size)
   assert(_size[2] > 0.0);
   mBoundingBox.setMin(-_size * 0.5);
   mBoundingBox.setMax(_size * 0.5);
-  initMeshes();
-  computeVolume();
+  updateVolume();
 }
 
 BoxShape::~BoxShape() {
 }
 
+//==============================================================================
+double BoxShape::computeVolume(const Eigen::Vector3d& size)
+{
+  return size[0] * size[1] * size[2];
+}
+
+//==============================================================================
+Eigen::Matrix3d BoxShape::computeInertia(const Eigen::Vector3d& size,
+                                         double mass)
+{
+  Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity();
+
+  inertia(0, 0) = mass / 12.0 * (std::pow(size[1], 2) + std::pow(size[2], 2));
+  inertia(1, 1) = mass / 12.0 * (std::pow(size[0], 2) + std::pow(size[2], 2));
+  inertia(2, 2) = mass / 12.0 * (std::pow(size[0], 2) + std::pow(size[1], 2));
+
+  return inertia;
+}
+
 void BoxShape::setSize(const Eigen::Vector3d& _size) {
   assert(_size[0] > 0.0);
   assert(_size[1] > 0.0);
   assert(_size[2] > 0.0);
   mSize = _size;
   mBoundingBox.setMin(-_size * 0.5);
   mBoundingBox.setMax(_size * 0.5);
-  computeVolume();
+  updateVolume();
 }
 
 const Eigen::Vector3d& BoxShape::getSize() const {
@@ -86,18 +104,16 @@ void BoxShape::draw(renderer::RenderInterface* _ri,
   _ri->popMatrix();
 }
 
-Eigen::Matrix3d BoxShape::computeInertia(double _mass) const {
-  Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity();
-  inertia(0, 0) = _mass / 12.0 * (mSize(1) * mSize(1) + mSize(2) * mSize(2));
-  inertia(1, 1) = _mass / 12.0 * (mSize(0) * mSize(0) + mSize(2) * mSize(2));
-  inertia(2, 2) = _mass / 12.0 * (mSize(0) * mSize(0) + mSize(1) * mSize(1));
-
-  return inertia;
+//==============================================================================
+Eigen::Matrix3d BoxShape::computeInertia(double mass) const
+{
+  return computeInertia(mSize, mass);
 }
 
-void BoxShape::computeVolume() {
-  // a * b * c
-  mVolume = mSize[0] * mSize[1] * mSize[2];
+//==============================================================================
+void BoxShape::updateVolume()
+{
+  mVolume = computeVolume(mSize);
 }
 
 }  // namespace dynamics

dart/dynamics/BoxShape.h

@@ -62,15 +62,22 @@ class BoxShape : public Shape {
             const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(),
             bool _default = true) const;
 
+  /// \brief Compute volume from given properties
+  static double computeVolume(const Eigen::Vector3d& size);
+
+  /// \brief Compute moments of inertia of a box
+  static Eigen::Matrix3d computeInertia(const Eigen::Vector3d& size,
+                                        double mass);
+
   // Documentation inherited.
-  virtual Eigen::Matrix3d computeInertia(double _mass) const;
+  Eigen::Matrix3d computeInertia(double mass) const override;
 
 protected:
   // Documentation inherited.
-  void computeVolume();
+  void updateVolume() override;
 
 private:
-  /// \brief Size of this box
+  /// \brief Side lengths of the box
   Eigen::Vector3d mSize;
 
 public:

dart/dynamics/CylinderShape.cpp

@@ -34,8 +34,8 @@
  *   POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <cmath>
 #include "dart/dynamics/CylinderShape.h"
-
 #include "dart/renderer/RenderInterface.h"
 
 namespace dart {
@@ -48,8 +48,7 @@ CylinderShape::CylinderShape(double _radius, double _height)
   assert(0.0 < _radius);
   assert(0.0 < _height);
   _updateBoundingBoxDim();
-  initMeshes();
-  computeVolume();
+  updateVolume();
 }
 
 double CylinderShape::getRadius() const {
@@ -60,7 +59,7 @@ void CylinderShape::setRadius(double _radius) {
   assert(0.0 < _radius);
   mRadius = _radius;
   _updateBoundingBoxDim();
-  computeVolume();
+  updateVolume();
 }
 
 double CylinderShape::getHeight() const {
@@ -71,7 +70,7 @@ void CylinderShape::setHeight(double _height) {
   assert(0.0 < _height);
   mHeight = _height;
   _updateBoundingBoxDim();
-  computeVolume();
+  updateVolume();
 }
 
 void CylinderShape::draw(renderer::RenderInterface* _ri,
@@ -89,19 +88,38 @@ void CylinderShape::draw(renderer::RenderInterface* _ri,
   _ri->popMatrix();
 }
 
-void CylinderShape::computeVolume() {
-  mVolume = DART_PI * mRadius * mRadius * mHeight;
+//==============================================================================
+double CylinderShape::computeVolume(double radius, double height)
+{
+  return DART_PI * std::pow(radius, 2) * height;
 }
 
-Eigen::Matrix3d CylinderShape::computeInertia(double _mass) const {
+//==============================================================================
+Eigen::Matrix3d CylinderShape::computeInertia(
+    double radius, double height, double mass)
+{
   Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero();
-  inertia(0, 0) = _mass * (3.0 * mRadius * mRadius + mHeight * mHeight) / 12.0;
+
+  inertia(0, 0) = mass * (3.0 * std::pow(radius, 2) + std::pow(height, 2))
+      / 12.0;
   inertia(1, 1) = inertia(0, 0);
-  inertia(2, 2) = 0.5 * _mass * mRadius * mRadius;
+  inertia(2, 2) = 0.5 * mass * radius * radius;
 
   return inertia;
 }
 
+//==============================================================================
+void CylinderShape::updateVolume()
+{
+  mVolume = computeVolume(mRadius, mHeight);
+}
+
+//==============================================================================
+Eigen::Matrix3d CylinderShape::computeInertia(double mass) const
+{
+  return computeInertia(mRadius, mHeight, mass);
+}
+
 void CylinderShape::_updateBoundingBoxDim() {
   mBoundingBox.setMin(Eigen::Vector3d(-mRadius, -mRadius, -mHeight * 0.5));
   mBoundingBox.setMax(Eigen::Vector3d(mRadius, mRadius, mHeight * 0.5));

dart/dynamics/CylinderShape.h

@@ -67,12 +67,19 @@ class CylinderShape : public Shape {
             const Eigen::Vector4d& _color = Eigen::Vector4d::Ones(),
             bool _useDefaultColor = true) const;
 
+  /// \brief Compute volume from given properties
+  static double computeVolume(double radius, double height);
+
+  /// \brief Compute moments of inertia of a cylinder
+  static Eigen::Matrix3d computeInertia(
+      double radius, double height, double mass);
+
   // Documentation inherited.
-  virtual Eigen::Matrix3d computeInertia(double _mass) const;
+  Eigen::Matrix3d computeInertia(double mass) const override;
 
 protected:
   // Documentation inherited.
-  virtual void computeVolume();
+  void updateVolume() override;
 
 private:
   /// \brief

dart/dynamics/EllipsoidShape.cpp

@@ -45,7 +45,6 @@ namespace dynamics {
 EllipsoidShape::EllipsoidShape(const Eigen::Vector3d& _size)
   : Shape(ELLIPSOID) {
   setSize(_size);
-  initMeshes();
 }
 
 EllipsoidShape::~EllipsoidShape() {
@@ -58,7 +57,7 @@ void EllipsoidShape::setSize(const Eigen::Vector3d& _size) {
   mSize = _size;
   mBoundingBox.setMin(-_size * 0.5);
   mBoundingBox.setMax(_size * 0.5);
-  computeVolume();
+  updateVolume();
 }
 
 const Eigen::Vector3d&EllipsoidShape::getSize() const {
@@ -82,25 +81,43 @@ void EllipsoidShape::draw(renderer::RenderInterface* _ri,
   _ri->popMatrix();
 }
 
-Eigen::Matrix3d EllipsoidShape::computeInertia(double _mass) const {
+//==============================================================================
+double EllipsoidShape::computeVolume(const Eigen::Vector3d& size)
+{
+  // 4/3* Pi* a/2* b/2* c/2
+  return DART_PI * size[0] * size[1] * size[2] / 6.0;
+}
+
+//==============================================================================
+Eigen::Matrix3d EllipsoidShape::computeInertia(
+    const Eigen::Vector3d& size, double mass)
+{
   Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity();
-  inertia(0, 0) = _mass / 20.0 * (mSize(1) * mSize(1) + mSize(2) * mSize(2));
-  inertia(1, 1) = _mass / 20.0 * (mSize(0) * mSize(0) + mSize(2) * mSize(2));
-  inertia(2, 2) = _mass / 20.0 * (mSize(0) * mSize(0) + mSize(1) * mSize(1));
+
+  inertia(0, 0) = mass / 20.0 * (std::pow(size[1], 2) + std::pow(size[2], 2));
+  inertia(1, 1) = mass / 20.0 * (std::pow(size[0], 2) + std::pow(size[2], 2));
+  inertia(2, 2) = mass / 20.0 * (std::pow(size[0], 2) + std::pow(size[1], 2));
 
   return inertia;
 }
 
+//==============================================================================
+Eigen::Matrix3d EllipsoidShape::computeInertia(double mass) const
+{
+  return computeInertia(mSize, mass);
+}
+
 bool EllipsoidShape::isSphere() const {
   if (mSize[0] == mSize[1] && mSize[1] == mSize[2])
     return true;
   else
     return false;
 }
 
-void EllipsoidShape::computeVolume() {
-  // 4/3* Pi* a/2* b/2* c/2
-  mVolume = DART_PI * mSize(0) * mSize(1) *mSize(2) / 6;
+//==============================================================================
+void EllipsoidShape::updateVolume()
+{
+  mVolume = computeVolume(mSize);
 }
 
 }  // namespace dynamics

dart/dynamics/EllipsoidShape.h

@@ -62,15 +62,22 @@ class EllipsoidShape : public Shape {
             const Eigen::Vector4d& _col = Eigen::Vector4d::Ones(),
             bool _useDefaultColor = true) const;
 
+  /// \brief Compute volume from given properties
+  static double computeVolume(const Eigen::Vector3d& size);
+
+  /// \brief Compute moments of inertia of a ellipsoid
+  static Eigen::Matrix3d computeInertia(
+      const Eigen::Vector3d& size, double mass);
+
   // Documentation inherited.
-  virtual Eigen::Matrix3d computeInertia(double _mass) const;
+  Eigen::Matrix3d computeInertia(double mass) const override;
 
   /// \brief True if (mDim[0] == mDim[1] == mDim[2]).
   bool isSphere(void) const;
 
 protected:
   // Documentation inherited.
-  void computeVolume();
+  void updateVolume() override;
 
 private:
   /// \brief Size of this ellipsoid

dart/dynamics/LineSegmentShape.cpp

@@ -56,7 +56,7 @@ LineSegmentShape::LineSegmentShape(float _thickness)
     mThickness = 1.0f;
   }
 
-  computeVolume();
+  updateVolume();
   mVariance = DYNAMIC_VERTICES;
 }
 
@@ -77,7 +77,7 @@ LineSegmentShape::LineSegmentShape(const Eigen::Vector3d& _v1,
 
   addVertex(_v1);
   addVertex(_v2);
-  computeVolume();
+  updateVolume();
   mVariance = DYNAMIC_VERTICES;
 }
 
@@ -362,9 +362,9 @@ Eigen::Matrix3d LineSegmentShape::computeInertia(double _mass) const
 }
 
 //==============================================================================
-void LineSegmentShape::computeVolume()
+void LineSegmentShape::updateVolume()
 {
-  mVolume = 0;
+  mVolume = 0.0;
 }
 
 } // namespace dynamics

dart/dynamics/LineSegmentShape.h

@@ -107,14 +107,14 @@ class LineSegmentShape : public Shape
 
   /// The returned inertia matrix will be like a very thin cylinder. The _mass
   /// will be evenly distributed across all lines.
-  virtual Eigen::Matrix3d computeInertia(double _mass) const override;
+  virtual Eigen::Matrix3d computeInertia(double mass) const override;
 
   // TODO(MXG): Consider supporting colors-per-vertex
 
 protected:
 
   // Documentation inherited
-  void computeVolume() override;
+  void updateVolume() override;
 
   /// Line thickness for rendering
   float mThickness;