Add accessors for diameters of EllipsoidShape

CHANGELOG.md

@@ -5,7 +5,7 @@
 * Math
 
   * Fixed Lemke LCP solver (#808 for DART 6): [#812](https://github.com/dartsim/dart/pull/812)
-  * Added accessors for radii of EllipsoidShape and deprecated accessors for size (diameters): [#829][#829](https://github.com/dartsim/dart/pull/xxx)
+  * Added accessors for diameters and radii of EllipsoidShape, and deprecated EllipsoidShape::get/setSize() [#829][#829](https://github.com/dartsim/dart/pull/xxx)
 
 * GUI
 

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -456,10 +456,10 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape(
     assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
 
     const auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
-    const Eigen::Vector3d& size = ellipsoid->getSize();
+    const Eigen::Vector3d& radii = ellipsoid->getRadii();
 
     bulletCollisionShape = createBulletEllipsoidMesh(
-          size[0], size[1], size[2]);
+          radii[0]*2.0, radii[1]*2.0, radii[2]*2.0);
   }
   else if (shape->is<CylinderShape>())
   {

dart/collision/dart/DARTCollide.cpp

@@ -1291,7 +1291,7 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
 
       return collideSphereSphere(o1, o2,
                                  sphere0->getRadius(), T1,
-                                 ellipsoid1->getSize()[0] * 0.5, T2,
+                                 ellipsoid1->getRadii()[0], T2,
                                  result);
     }
   }
@@ -1324,7 +1324,7 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
 
       return collideBoxSphere(o1, o2,
                               box0->getSize(), T1,
-                              ellipsoid1->getSize()[0] * 0.5, T2,
+                              ellipsoid1->getRadii()[0], T2,
                               result);
     }
   }
@@ -1339,7 +1339,7 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
           = static_cast<const dynamics::SphereShape*>(shape2.get());
 
       return collideSphereSphere(o1, o2,
-                                 ellipsoid0->getSize()[0] * 0.5, T1,
+                                 ellipsoid0->getRadii()[0], T1,
                                  sphere1->getRadius(), T2,
                                  result);
     }
@@ -1349,7 +1349,7 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
           = static_cast<const dynamics::BoxShape*>(shape2.get());
 
       return collideSphereBox(o1, o2,
-                              ellipsoid0->getSize()[0] * 0.5, T1,
+                              ellipsoid0->getRadii()[0], T1,
                               box1->getSize(), T2,
                               result);
     }
@@ -1359,8 +1359,8 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
           = static_cast<const dynamics::EllipsoidShape*>(shape2.get());
 
       return collideSphereSphere(o1, o2,
-                                 ellipsoid0->getSize()[0] * 0.5, T1,
-                                 ellipsoid1->getSize()[0] * 0.5, T2,
+                                 ellipsoid0->getRadii()[0], T1,
+                                 ellipsoid1->getRadii()[0], T2,
                                  result);
     }
   }

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -463,13 +463,16 @@ fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ)
 
   for (int i = 0; i < 112; i++)
   {
-    p1 = fcl::Vec3f(v[f[i][0]][0] * _sizeX,
+    p1 = fcl::Vec3f(
+        v[f[i][0]][0] * _sizeX,
         v[f[i][0]][1] * _sizeY,
         v[f[i][0]][2] * _sizeZ);
-    p2 = fcl::Vec3f(v[f[i][1]][0] * _sizeX,
+    p2 = fcl::Vec3f(
+        v[f[i][1]][0] * _sizeX,
         v[f[i][1]][1] * _sizeY,
         v[f[i][1]][2] * _sizeZ);
-    p3 = fcl::Vec3f(v[f[i][2]][0] * _sizeX,
+    p3 = fcl::Vec3f(
+        v[f[i][2]][0] * _sizeX,
         v[f[i][2]][1] * _sizeY,
         v[f[i][2]][2] * _sizeZ);
 
@@ -951,19 +954,21 @@ FCLCollisionDetector::createFCLCollisionGeometry(
     assert(dynamic_cast<const EllipsoidShape*>(shape.get()));
 
     auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
-    const Eigen::Vector3d& size = ellipsoid->getSize();
+    const Eigen::Vector3d& radii = ellipsoid->getRadii();
 
     if (FCLCollisionDetector::PRIMITIVE == type)
     {
 #if FCL_VERSION_AT_LEAST(0,4,0)
-      geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5));
+      geom = new fcl::Ellipsoid(FCLTypes::convertVector3(radii));
 #else
-      geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
+      geom = createEllipsoid<fcl::OBBRSS>(
+          radii[0]*2.0, radii[1]*2.0, radii[2]*2.0);
 #endif
     }
     else
     {
-      geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
+      geom = createEllipsoid<fcl::OBBRSS>(
+          radii[0]*2.0, radii[1]*2.0, radii[2]*2.0);
     }
   }
   else if (CylinderShape::getStaticType() == shapeType)

dart/dynamics/EllipsoidShape.cpp

@@ -40,7 +40,7 @@ namespace dynamics {
 EllipsoidShape::EllipsoidShape(const Eigen::Vector3d& diameters)
   : Shape(ELLIPSOID)
 {
-  setSize(diameters);
+  setDiameters(diameters);
 }
 
 //==============================================================================
@@ -62,60 +62,87 @@ const std::string& EllipsoidShape::getStaticType()
   return type;
 }
 
-void EllipsoidShape::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);
+//==============================================================================
+void EllipsoidShape::setSize(const Eigen::Vector3d& diameters)
+{
+  setDiameters(diameters);
+}
+
+//==============================================================================
+const Eigen::Vector3d& EllipsoidShape::getSize() const
+{
+  return getDiameters();
+}
+
+//==============================================================================
+void EllipsoidShape::setDiameters(const Eigen::Vector3d& diameters)
+{
+  assert(diameters[0] > 0.0);
+  assert(diameters[1] > 0.0);
+  assert(diameters[2] > 0.0);
+
+  mDiameters = diameters;
+
+  mBoundingBox.setMin(-diameters * 0.5);
+  mBoundingBox.setMax(diameters * 0.5);
+
   updateVolume();
 }
 
-const Eigen::Vector3d&EllipsoidShape::getSize() const {
-  return mSize;
+//==============================================================================
+const Eigen::Vector3d& EllipsoidShape::getDiameters() const
+{
+  return mDiameters;
 }
 
 //==============================================================================
 void EllipsoidShape::setRadii(const Eigen::Vector3d& radii)
 {
-  mSize = radii * 2.0;
+  mDiameters = radii * 2.0;
 }
 
 //==============================================================================
 const Eigen::Vector3d EllipsoidShape::getRadii() const
 {
-  return mSize / 2.0;
+  return mDiameters / 2.0;
 }
 
 //==============================================================================
-double EllipsoidShape::computeVolume(const Eigen::Vector3d& size)
+double EllipsoidShape::computeVolume(const Eigen::Vector3d& diameters)
 {
   // 4/3* Pi* a/2* b/2* c/2
-  return math::constantsd::pi() * size[0] * size[1] * size[2] / 6.0;
+  return math::constantsd::pi()
+      * diameters[0] * diameters[1] * diameters[2] / 6.0;
 }
 
 //==============================================================================
 Eigen::Matrix3d EllipsoidShape::computeInertia(
-    const Eigen::Vector3d& size, double mass)
+    const Eigen::Vector3d& diameters, double mass)
 {
   Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity();
 
-  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));
+  const auto coeff = mass / 20.0;
+  const auto AA = std::pow(diameters[0], 2);
+  const auto BB = std::pow(diameters[1], 2);
+  const auto CC = std::pow(diameters[2], 2);
+
+  inertia(0, 0) = coeff*(BB + CC);
+  inertia(1, 1) = coeff*(AA + CC);
+  inertia(2, 2) = coeff*(AA + BB);
 
   return inertia;
 }
 
 //==============================================================================
 Eigen::Matrix3d EllipsoidShape::computeInertia(double mass) const
 {
-  return computeInertia(mSize, mass);
+  return computeInertia(mDiameters, mass);
 }
 
-bool EllipsoidShape::isSphere() const {
-  if (mSize[0] == mSize[1] && mSize[1] == mSize[2])
+//==============================================================================
+bool EllipsoidShape::isSphere() const
+{
+  if (mDiameters[0] == mDiameters[1] && mDiameters[1] == mDiameters[2])
     return true;
   else
     return false;
@@ -124,7 +151,7 @@ bool EllipsoidShape::isSphere() const {
 //==============================================================================
 void EllipsoidShape::updateVolume()
 {
-  mVolume = computeVolume(mSize);
+  mVolume = computeVolume(mDiameters);
 }
 
 }  // namespace dynamics

dart/dynamics/EllipsoidShape.hpp

@@ -37,7 +37,8 @@
 namespace dart {
 namespace dynamics {
 
-class EllipsoidShape : public Shape {
+class EllipsoidShape : public Shape
+{
 public:
   /// \brief Constructor.
   explicit EllipsoidShape(const Eigen::Vector3d& diameters);
@@ -54,41 +55,51 @@ class EllipsoidShape : public Shape {
   static const std::string& getStaticType();
 
   /// \brief Set diameters of this ellipsoid.
-  /// \deprecated Deprecated in 6.2. Please use setRadii() instead.
+  /// \deprecated Deprecated in 6.2. Please use setDiameters() instead.
   DART_DEPRECATED(6.2)
-  void setSize(const Eigen::Vector3d& _size);
+  void setSize(const Eigen::Vector3d& diameters);
 
   /// \brief Get diameters of this ellipsoid.
-  /// \deprecated Deprecated in 6.2. Please use getRadii() instead.
+  /// \deprecated Deprecated in 6.2. Please use getDiameters() instead.
   DART_DEPRECATED(6.2)
   const Eigen::Vector3d& getSize() const;
 
+  /// \brief Set diameters of this ellipsoid.
+  void setDiameters(const Eigen::Vector3d& diameters);
+
+  /// \brief Get diameters of this ellipsoid.
+  const Eigen::Vector3d& getDiameters() const;
+
   /// Set radii of this ellipsoid.
   void setRadii(const Eigen::Vector3d& radii);
 
   /// Get radii of this ellipsoid.
   const Eigen::Vector3d getRadii() const;
 
   /// \brief Compute volume from given properties
-  static double computeVolume(const Eigen::Vector3d& size);
+  static double computeVolume(const Eigen::Vector3d& diameters);
+  // TODO(JS): In order to follow the commonly used convention, change to take
+  // radii instead of diameters in DART 7.
 
   /// \brief Compute moments of inertia of a ellipsoid
   static Eigen::Matrix3d computeInertia(
-      const Eigen::Vector3d& size, double mass);
+      const Eigen::Vector3d& diameters, double mass);
+  // TODO(JS): In order to follow the commonly used convention, change to take
+  // radii instead of diameters in DART 7.
 
   // Documentation inherited.
   Eigen::Matrix3d computeInertia(double mass) const override;
 
-  /// \brief True if (mDim[0] == mDim[1] == mDim[2]).
+  /// \brief True if all the radii are exactly eqaul.
   bool isSphere(void) const;
 
 protected:
   // Documentation inherited.
   void updateVolume() override;
 
 private:
-  /// \brief Size of this ellipsoid
-  Eigen::Vector3d mSize;
+  /// \brief Diameters of this ellipsoid
+  Eigen::Vector3d mDiameters;
 
 public:
   // To get byte-aligned Eigen vectors

dart/gui/OpenGLRenderInterface.cpp

@@ -157,8 +157,8 @@ void OpenGLRenderInterface::drawSphere(double radius)
   gluSphere(quadObj, radius, slices, stacks);
 }
 
-void OpenGLRenderInterface::drawEllipsoid(const Eigen::Vector3d& _size) {
-    glScaled(_size(0), _size(1), _size(2));
+void OpenGLRenderInterface::drawEllipsoid(const Eigen::Vector3d& _diameters) {
+    glScaled(_diameters(0), _diameters(1), _diameters(2));
 
     drawSphere(0.5);
 }

dart/gui/OpenGLRenderInterface.hpp

@@ -81,7 +81,7 @@ class OpenGLRenderInterface : public RenderInterface {
     GLuint compileList(const Eigen::Vector3d& _scale, const aiScene* _mesh);
 
     void drawSphere(double _radius) override;
-    void drawEllipsoid(const Eigen::Vector3d& _size) override;
+    void drawEllipsoid(const Eigen::Vector3d& _diameters) override;
     void drawCube(const Eigen::Vector3d& _size) override;
     void drawCylinder(double _radius, double _height) override;
     void drawCapsule(double radius, double height) override;

dart/gui/SimWindow.cpp

@@ -420,7 +420,7 @@ void SimWindow::drawShape(const dynamics::Shape* shape,
   else if (shape->is<EllipsoidShape>())
   {
     const auto* ellipsoid = static_cast<const EllipsoidShape*>(shape);
-    mRI->drawEllipsoid(ellipsoid->getSize());
+    mRI->drawEllipsoid(ellipsoid->getDiameters());
   }
   else if (shape->is<CylinderShape>())
   {

dart/gui/osg/render/EllipsoidShapeNode.cpp

@@ -127,7 +127,8 @@ void EllipsoidShapeNode::extractData(bool firstTime)
   {
     Eigen::Matrix4d S(Eigen::Matrix4d::Zero());
     const Eigen::Vector3d& s =
-       mEllipsoidShape->getSize()/smallestComponent(mEllipsoidShape->getSize());
+        mEllipsoidShape->getDiameters()
+        /smallestComponent(mEllipsoidShape->getDiameters());
     S(0,0) = s[0]; S(1,1) = s[1]; S(2,2) = s[2]; S(3,3) = 1.0;
     setMatrix(eigToOsgMatrix(S));
   }
@@ -213,7 +214,7 @@ void EllipsoidShapeDrawable::refresh(bool firstTime)
   {
     ::osg::ref_ptr<::osg::Sphere> osg_shape = nullptr;
     osg_shape = new ::osg::Sphere(::osg::Vec3(0,0,0),
-                          0.5*smallestComponent(mEllipsoidShape->getSize()));
+                          smallestComponent(mEllipsoidShape->getRadii()));
 
     setShape(osg_shape);
     dirtyDisplayList();