SphereShape

CHANGELOG.md

@@ -5,10 +5,12 @@
 * Dynamics
 
   * Added `computeLagrangian()` to `MetaSkeleton` and `BodyNode`: [#746](https://github.com/dartsim/dart/pull/746)
+  * Added `SphereShape`: [#745](https://github.com/dartsim/dart/pull/745)
 
 * Misc improvements and bug fixes
 
   * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)
+  * Fixed typo: [#756](https://github.com/dartsim/dart/pull/756), [#755](https://github.com/dartsim/dart/pull/755)
 
 ### DART 6.0.1 (2016-XX-XX)
 

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -44,6 +44,7 @@
 #include "dart/collision/bullet/BulletCollisionGroup.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
 #include "dart/dynamics/Shape.hpp"
+#include "dart/dynamics/SphereShape.hpp"
 #include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
@@ -391,6 +392,7 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape(
     const dynamics::ConstShapePtr& shape)
 {
   using dynamics::Shape;
+  using dynamics::SphereShape;
   using dynamics::BoxShape;
   using dynamics::EllipsoidShape;
   using dynamics::CylinderShape;
@@ -401,7 +403,16 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape(
   const auto& shapeType = shape->getType();
   btCollisionShape* bulletCollisionShape = nullptr;
 
-  if (BoxShape::getStaticType() == shapeType)
+  if (SphereShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const SphereShape*>(shape.get()));
+
+    const auto sphere = static_cast<const SphereShape*>(shape.get());
+    const auto radius = sphere->getRadius();
+
+    bulletCollisionShape = new btSphereShape(radius);
+  }
+  else if (BoxShape::getStaticType() == shapeType)
   {
     assert(dynamic_cast<const BoxShape*>(shape.get()));
 
@@ -417,15 +428,8 @@ btCollisionShape* BulletCollisionDetector::createBulletCollisionShape(
     const auto ellipsoid = static_cast<const EllipsoidShape*>(shape.get());
     const Eigen::Vector3d& size = ellipsoid->getSize();
 
-    if (ellipsoid->isSphere())
-    {
-      bulletCollisionShape = new btSphereShape(size[0] * 0.5);
-    }
-    else
-    {
-      bulletCollisionShape = createBulletEllipsoidMesh(
-            size[0], size[1], size[2]);
-    }
+    bulletCollisionShape = createBulletEllipsoidMesh(
+          size[0], size[1], size[2]);
   }
   else if (CylinderShape::getStaticType() == shapeType)
   {

dart/collision/dart/DARTCollide.cpp

@@ -35,6 +35,7 @@
 #include <memory>
 
 #include "dart/math/Helpers.hpp"
+#include "dart/dynamics/SphereShape.hpp"
 #include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
@@ -1262,11 +1263,51 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
   const Eigen::Isometry3d& T1 = o1->getTransform();
   const Eigen::Isometry3d& T2 = o2->getTransform();
 
-  if (dynamics::BoxShape::getStaticType() == shapeType1)
+  if (dynamics::SphereShape::getStaticType() == shapeType1)
+  {
+    const auto* sphere0
+        = static_cast<const dynamics::SphereShape*>(shape1.get());
+
+    if (dynamics::SphereShape::getStaticType() == shapeType2)
+    {
+      const auto* sphere1
+          = static_cast<const dynamics::SphereShape*>(shape2.get());
+
+      return collideSphereSphere(
+            o1, o2, sphere0->getRadius(), T1, sphere1->getRadius(), T2, result);
+    }
+    else if (dynamics::BoxShape::getStaticType() == shapeType2)
+    {
+      const auto* box1
+          = static_cast<const dynamics::BoxShape*>(shape2.get());
+
+      return collideSphereBox(
+            o1, o2, sphere0->getRadius(), T1, box1->getSize(), T2, result);
+    }
+    else if (dynamics::EllipsoidShape::getStaticType() == shapeType2)
+    {
+      const auto* ellipsoid1
+          = static_cast<const dynamics::EllipsoidShape*>(shape2.get());
+
+      return collideSphereSphere(o1, o2,
+                                 sphere0->getRadius(), T1,
+                                 ellipsoid1->getSize()[0] * 0.5, T2,
+                                 result);
+    }
+  }
+  else if (dynamics::BoxShape::getStaticType() == shapeType1)
   {
     const auto* box0 = static_cast<const dynamics::BoxShape*>(shape1.get());
 
-    if (dynamics::BoxShape::getStaticType() == shapeType2)
+    if (dynamics::SphereShape::getStaticType() == shapeType2)
+    {
+      const auto* sphere1
+          = static_cast<const dynamics::SphereShape*>(shape2.get());
+
+      return collideBoxSphere(
+            o1, o2, box0->getSize(), T1, sphere1->getRadius(), T2, result);
+    }
+    else if (dynamics::BoxShape::getStaticType() == shapeType2)
     {
       const auto* box1
           = static_cast<const dynamics::BoxShape*>(shape2.get());
@@ -1292,7 +1333,17 @@ int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result)
     const auto* ellipsoid0
         = static_cast<const dynamics::EllipsoidShape*>(shape1.get());
 
-    if (dynamics::BoxShape::getStaticType() == shapeType2)
+    if (dynamics::SphereShape::getStaticType() == shapeType2)
+    {
+      const auto* sphere1
+          = static_cast<const dynamics::SphereShape*>(shape2.get());
+
+      return collideSphereSphere(o1, o2,
+                                 ellipsoid0->getSize()[0] * 0.5, T1,
+                                 sphere1->getRadius(), T2,
+                                 result);
+    }
+    else if (dynamics::BoxShape::getStaticType() == shapeType2)
     {
       const auto* box1
           = static_cast<const dynamics::BoxShape*>(shape2.get());

dart/collision/dart/DARTCollisionDetector.cpp

@@ -37,6 +37,7 @@
 #include "dart/collision/dart/DARTCollisionObject.hpp"
 #include "dart/collision/dart/DARTCollisionGroup.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
+#include "dart/dynamics/SphereShape.hpp"
 #include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 
@@ -239,6 +240,9 @@ void warnUnsupportedShapeType(const dynamics::ShapeFrame* shapeFrame)
   const auto& shape = shapeFrame->getShape();
   const auto& shapeType = shape->getType();
 
+  if (shapeType == dynamics::SphereShape::getStaticType())
+    return;
+
   if (shapeType == dynamics::BoxShape::getStaticType())
     return;
 

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -50,6 +50,7 @@
 #include "dart/collision/fcl/tri_tri_intersection_test.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
 #include "dart/dynamics/Shape.hpp"
+#include "dart/dynamics/SphereShape.hpp"
 #include "dart/dynamics/BoxShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
@@ -808,6 +809,7 @@ FCLCollisionDetector::createFCLCollisionGeometry(
     const FCLCollisionGeometryDeleter& deleter)
 {
   using dynamics::Shape;
+  using dynamics::SphereShape;
   using dynamics::BoxShape;
   using dynamics::EllipsoidShape;
   using dynamics::CylinderShape;
@@ -818,7 +820,19 @@ FCLCollisionDetector::createFCLCollisionGeometry(
   fcl::CollisionGeometry* geom = nullptr;
   const auto& shapeType = shape->getType();
 
-  if (BoxShape::getStaticType() == shapeType)
+  if (SphereShape::getStaticType() == shapeType)
+  {
+    assert(dynamic_cast<const SphereShape*>(shape.get()));
+
+    auto* sphere = static_cast<const SphereShape*>(shape.get());
+    const auto radius = sphere->getRadius();
+
+    if (FCLCollisionDetector::PRIMITIVE == type)
+      geom = new fcl::Sphere(radius);
+    else
+      geom = createEllipsoid<fcl::OBBRSS>(radius*2.0, radius*2.0, radius*2.0);
+  }
+  else if (BoxShape::getStaticType() == shapeType)
   {
     assert(dynamic_cast<const BoxShape*>(shape.get()));
 
@@ -839,18 +853,11 @@ FCLCollisionDetector::createFCLCollisionGeometry(
 
     if (FCLCollisionDetector::PRIMITIVE == type)
     {
-      if (ellipsoid->isSphere())
-      {
-        geom = new fcl::Sphere(size[0] * 0.5);
-      }
-      else
-      {
 #if FCL_VERSION_AT_LEAST(0,4,0)
-        geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5));
+      geom = new fcl::Ellipsoid(FCLTypes::convertVector3(size * 0.5));
 #else
-        geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
+      geom = createEllipsoid<fcl::OBBRSS>(size[0], size[1], size[2]);
 #endif
-      }
     }
     else
     {

dart/dynamics/Shape.hpp

@@ -50,6 +50,7 @@ class Shape : public virtual common::Subject
 
   /// \deprecated Deprecated in 6.1. Please use getType() instead.
   enum ShapeType {
+    SPHERE,
     BOX,
     ELLIPSOID,
     CYLINDER,

dart/dynamics/SoftBodyNode.cpp

@@ -2509,6 +2509,26 @@ void SoftBodyNodeHelper::setSinglePointMass(SoftBodyNode* _softBodyNode,
                                  _dampingCoeff));
 }
 
+//==============================================================================
+SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeSphereProperties(
+    double _radius,
+    std::size_t _nSlices,
+    std::size_t _nStacks,
+    double _totalMass,
+    double _vertexStiffness,
+    double _edgeStiffness,
+    double _dampingCoeff)
+{
+  return makeEllipsoidProperties(
+        Eigen::Vector3d::Constant(_radius*2.0),
+        _nSlices,
+        _nStacks,
+        _totalMass,
+        _vertexStiffness,
+        _edgeStiffness,
+        _dampingCoeff);
+}
+
 //==============================================================================
 SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties(
     const Eigen::Vector3d& _size,

dart/dynamics/SoftBodyNode.hpp

@@ -393,6 +393,16 @@ class SoftBodyNodeHelper
       double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
       double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
 
+  /// Create a Properties struct for an Sphere-shaped SoftBodyNode
+  static SoftBodyNode::UniqueProperties makeSphereProperties(
+      double                 _radius,
+      std::size_t            _nSlices,
+      std::size_t            _nStacks,
+      double                 _totalMass,
+      double                 _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
+      double                 _edgeStiffness   = DART_DEFAULT_EDGE_STIFNESS,
+      double                 _dampingCoeff    = DART_DEFAULT_DAMPING_COEFF);
+
   /// Create a Properties struct for an Ellipsoid-shaped SoftBodyNode
   static SoftBodyNode::UniqueProperties makeEllipsoidProperties(
       const Eigen::Vector3d& _size,

dart/dynamics/SphereShape.cpp

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2016, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Humanoid Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2016, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "dart/dynamics/SphereShape.hpp"
+
+#include "dart/math/Helpers.hpp"
+
+namespace dart {
+namespace dynamics {
+
+//==============================================================================
+SphereShape::SphereShape(double radius)
+  : Shape(SPHERE)
+{
+  setRadius(radius);
+}
+
+//==============================================================================
+SphereShape::~SphereShape()
+{
+  // Do nothing
+}
+
+//==============================================================================
+const std::string& SphereShape::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& SphereShape::getStaticType()
+{
+  static const std::string type("SphereShape");
+  return type;
+}
+
+//==============================================================================
+void SphereShape::setRadius(double radius)
+{
+  assert(radius > 0.0);
+
+  mRadius = radius;
+
+  mBoundingBox.setMin(Eigen::Vector3d::Constant(-radius));
+  mBoundingBox.setMax(Eigen::Vector3d::Constant(radius));
+
+  updateVolume();
+}
+
+//==============================================================================
+double SphereShape::getRadius() const
+{
+  return mRadius;
+}
+
+//==============================================================================
+double SphereShape::computeVolume(double radius)
+{
+  return math::constantsd::pi() * 4.0 / 3.0 * std::pow(radius, 3) ;
+}
+
+//==============================================================================
+Eigen::Matrix3d SphereShape::computeInertia(double radius, double mass)
+{
+  Eigen::Matrix3d inertia = Eigen::Matrix3d::Identity();
+
+  inertia(0, 0) = 2.0 / 5.0 * mass * std::pow(radius, 2);
+  inertia(1, 1) = inertia(0, 0);
+  inertia(2, 2) = inertia(0, 0);
+
+  return inertia;
+}
+
+//==============================================================================
+Eigen::Matrix3d SphereShape::computeInertia(double mass) const
+{
+  return computeInertia(mRadius, mass);
+}
+
+//==============================================================================
+void SphereShape::updateVolume()
+{
+  mVolume = computeVolume(mRadius);
+}
+
+}  // namespace dynamics
+}  // namespace dart