geometric_shapes.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  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.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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 OWNER 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.
 */
 
#ifndef COAL_GEOMETRIC_SHAPES_H
#define COAL_GEOMETRIC_SHAPES_H
 
#include <vector>
#include <memory>
 
#include <boost/math/constants/constants.hpp>
 
#include "coal/collision_object.h"
#include "coal/data_types.h"
 
#ifdef COAL_HAS_QHULL
namespace orgQhull {
class Qhull;
}
#endif
 
namespace coal {
 
class COAL_DLLAPI ShapeBase : public CollisionGeometry {
 public:
  ShapeBase() {}
 
  ShapeBase(const ShapeBase& other)
      : CollisionGeometry(other),
        m_swept_sphere_radius(other.m_swept_sphere_radius) {}
 
  ShapeBase& operator=(const ShapeBase& other) = default;
 
  virtual ~ShapeBase() {};
 
  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
 
  void setSweptSphereRadius(Scalar radius) {
    if (radius < 0) {
      COAL_THROW_PRETTY("Swept-sphere radius must be positive.",
                        std::invalid_argument);
    }
    this->m_swept_sphere_radius = radius;
  }
 
  Scalar getSweptSphereRadius() const { return this->m_swept_sphere_radius; }
 
 protected:
  Scalar m_swept_sphere_radius{0};
};
 
 
class COAL_DLLAPI TriangleP : public ShapeBase {
 public:
  TriangleP() {};
 
  TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_)
      : ShapeBase(), a(a_), b(b_), c(c_) {}
 
  TriangleP(const TriangleP& other)
      : ShapeBase(other), a(other.a), b(other.b), c(other.c) {}
 
  virtual TriangleP* clone() const { return new TriangleP(*this); };
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
 
  //  std::pair<ShapeBase*, Transform3s> inflated(const Scalar value) const
  //  {
  //    if (value == 0) return std::make_pair(new TriangleP(*this),
  //    Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize();
  //    BC.normalize();
  //    CA.normalize();
  //
  //    Vec3s new_a(a + value * Vec3s(-AB + CA).normalized());
  //    Vec3s new_b(b + value * Vec3s(-BC + AB).normalized());
  //    Vec3s new_c(c + value * Vec3s(-CA + BC).normalized());
  //
  //    return std::make_pair(new TriangleP(new_a, new_b, new_c),
  //    Transform3s());
  //  }
  //
  //  Scalar minInflationValue() const
  //  {
  //    return (std::numeric_limits<Scalar>::max)(); // TODO(jcarpent):
  //    implement
  //  }
 
  Vec3s a, b, c;
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const TriangleP* other_ptr = dynamic_cast<const TriangleP*>(&_other);
    if (other_ptr == nullptr) return false;
    const TriangleP& other = *other_ptr;
 
    return a == other.a && b == other.b && c == other.c &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Box : public ShapeBase {
 public:
  Box(Scalar x, Scalar y, Scalar z)
      : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
 
  Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {}
 
  Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
 
  Box& operator=(const Box& other) {
    if (this == &other) return *this;
 
    this->halfSide = other.halfSide;
    return *this;
  }
 
  virtual Box* clone() const { return new Box(*this); };
 
  Box() {}
 
  Vec3s halfSide;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_BOX; }
 
  Scalar computeVolume() const { return 8 * halfSide.prod(); }
 
  Matrix3s computeMomentofInertia() const {
    Scalar V = computeVolume();
    Vec3s s(halfSide.cwiseAbs2() * V);
    return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
  }
 
  Scalar minInflationValue() const { return -halfSide.minCoeff(); }
 
  std::pair<Box, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value << ") "
                                  << "is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))),
                          Transform3s());
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Box* other_ptr = dynamic_cast<const Box*>(&_other);
    if (other_ptr == nullptr) return false;
    const Box& other = *other_ptr;
 
    return halfSide == other.halfSide &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Sphere : public ShapeBase {
 public:
  Sphere() {}
 
  explicit Sphere(Scalar radius_) : ShapeBase(), radius(radius_) {}
 
  Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
 
  virtual Sphere* clone() const { return new Sphere(*this); };
 
  Scalar radius;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
 
  Matrix3s computeMomentofInertia() const {
    Scalar I = Scalar(0.4) * radius * radius * computeVolume();
    return I * Matrix3s::Identity();
  }
 
  Scalar computeVolume() const {
    return 4 * boost::math::constants::pi<Scalar>() * radius * radius * radius /
           3;
  }
 
  Scalar minInflationValue() const { return -radius; }
 
  std::pair<Sphere, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Sphere(radius + value), Transform3s());
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Sphere* other_ptr = dynamic_cast<const Sphere*>(&_other);
    if (other_ptr == nullptr) return false;
    const Sphere& other = *other_ptr;
 
    return radius == other.radius &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Ellipsoid : public ShapeBase {
 public:
  Ellipsoid() {}
 
  Ellipsoid(Scalar rx, Scalar ry, Scalar rz) : ShapeBase(), radii(rx, ry, rz) {}
 
  explicit Ellipsoid(const Vec3s& radii) : radii(radii) {}
 
  Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
 
  virtual Ellipsoid* clone() const { return new Ellipsoid(*this); };
 
  Vec3s radii;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
 
  Matrix3s computeMomentofInertia() const {
    Scalar V = computeVolume();
    Scalar a2 = V * radii[0] * radii[0];
    Scalar b2 = V * radii[1] * radii[1];
    Scalar c2 = V * radii[2] * radii[2];
    Scalar alpha = Scalar(0.2);
    return (Matrix3s() << alpha * (b2 + c2), 0, 0, 0, alpha * (a2 + c2), 0, 0,
            0, alpha * (a2 + b2))
        .finished();
  }
 
  Scalar computeVolume() const {
    return 4 * boost::math::constants::pi<Scalar>() * radii[0] * radii[1] *
           radii[2] / 3;
  }
 
  Scalar minInflationValue() const { return -radii.minCoeff(); }
 
  std::pair<Ellipsoid, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)),
                          Transform3s());
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Ellipsoid* other_ptr = dynamic_cast<const Ellipsoid*>(&_other);
    if (other_ptr == nullptr) return false;
    const Ellipsoid& other = *other_ptr;
 
    return radii == other.radii &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Capsule : public ShapeBase {
 public:
  Capsule() {}
 
  Capsule(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
    halfLength = lz_ / 2;
  }
 
  Capsule(const Capsule& other)
      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
 
  virtual Capsule* clone() const { return new Capsule(*this); };
 
  Scalar radius;
 
  Scalar halfLength;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
 
  Scalar computeVolume() const {
    return boost::math::constants::pi<Scalar>() * radius * radius *
           ((halfLength * 2) + radius * 4 / Scalar(3));
  }
 
  Matrix3s computeMomentofInertia() const {
    Scalar v_cyl = radius * radius * (halfLength * 2) *
                   boost::math::constants::pi<Scalar>();
    Scalar v_sph = radius * radius * radius *
                   boost::math::constants::pi<Scalar>() * 4 / Scalar(3);
 
    Scalar h2 = halfLength * halfLength;
    Scalar r2 = radius * radius;
    Scalar ix =
        v_cyl * (h2 / Scalar(3) + r2 / Scalar(4)) +
        v_sph * (Scalar(0.4) * r2 + h2 + Scalar(0.75) * radius * halfLength);
    Scalar iz = (Scalar(0.5) * v_cyl + Scalar(0.4) * v_sph) * radius * radius;
 
    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
  }
 
  Scalar minInflationValue() const { return -radius; }
 
  std::pair<Capsule, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Capsule(radius + value, 2 * halfLength),
                          Transform3s());
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Capsule* other_ptr = dynamic_cast<const Capsule*>(&_other);
    if (other_ptr == nullptr) return false;
    const Capsule& other = *other_ptr;
 
    return radius == other.radius && halfLength == other.halfLength &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Cone : public ShapeBase {
 public:
  Cone() {}
 
  Cone(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
    halfLength = lz_ / 2;
  }
 
  Cone(const Cone& other)
      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
 
  virtual Cone* clone() const { return new Cone(*this); };
 
  Scalar radius;
 
  Scalar halfLength;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_CONE; }
 
  Scalar computeVolume() const {
    return boost::math::constants::pi<Scalar>() * radius * radius *
           (halfLength * 2) / 3;
  }
 
  Matrix3s computeMomentofInertia() const {
    Scalar V = computeVolume();
    Scalar ix =
        V * (Scalar(0.4) * halfLength * halfLength + 3 * radius * radius / 20);
    Scalar iz = Scalar(0.3) * V * radius * radius;
 
    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
  }
 
  Vec3s computeCOM() const { return Vec3s(0, 0, -Scalar(0.5) * halfLength); }
 
  Scalar minInflationValue() const { return -(std::min)(radius, halfLength); }
 
  std::pair<Cone, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
 
    // tan(alpha) = 2*halfLength/radius;
    const Scalar tan_alpha = 2 * halfLength / radius;
    const Scalar sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
    const Scalar top_inflation = value / sin_alpha;
    const Scalar bottom_inflation = value;
 
    const Scalar new_lz = 2 * halfLength + top_inflation + bottom_inflation;
    const Scalar new_cz = (top_inflation + bottom_inflation) / Scalar(2);
    const Scalar new_radius = new_lz / tan_alpha;
 
    return std::make_pair(Cone(new_radius, new_lz),
                          Transform3s(Vec3s(0., 0., new_cz)));
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Cone* other_ptr = dynamic_cast<const Cone*>(&_other);
    if (other_ptr == nullptr) return false;
    const Cone& other = *other_ptr;
 
    return radius == other.radius && halfLength == other.halfLength &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Cylinder : public ShapeBase {
 public:
  Cylinder() {}
 
  Cylinder(Scalar radius_, Scalar lz_) : ShapeBase(), radius(radius_) {
    halfLength = lz_ / 2;
  }
 
  Cylinder(const Cylinder& other)
      : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
 
  Cylinder& operator=(const Cylinder& other) {
    if (this == &other) return *this;
 
    this->radius = other.radius;
    this->halfLength = other.halfLength;
    return *this;
  }
 
  virtual Cylinder* clone() const { return new Cylinder(*this); };
 
  Scalar radius;
 
  Scalar halfLength;
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
 
  Scalar computeVolume() const {
    return boost::math::constants::pi<Scalar>() * radius * radius *
           (halfLength * 2);
  }
 
  Matrix3s computeMomentofInertia() const {
    Scalar V = computeVolume();
    Scalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
    Scalar iz = V * radius * radius / 2;
    return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
  }
 
  Scalar minInflationValue() const { return -(std::min)(radius, halfLength); }
 
  std::pair<Cylinder, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)),
                          Transform3s());
  }
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Cylinder* other_ptr = dynamic_cast<const Cylinder*>(&_other);
    if (other_ptr == nullptr) return false;
    const Cylinder& other = *other_ptr;
 
    return radius == other.radius && halfLength == other.halfLength &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI ConvexBase : public ShapeBase {
 public:
  static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3s>>& points,
                                unsigned int num_points, bool keepTriangles,
                                const char* qhullCommand = NULL);
 
  // TODO(louis): put this method in private sometime in the future.
  COAL_DEPRECATED static ConvexBase* convexHull(
      const Vec3s* points, unsigned int num_points, bool keepTriangles,
      const char* qhullCommand = NULL);
 
  virtual ~ConvexBase();
 
  virtual ConvexBase* clone() const { return new ConvexBase(*this); }
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
 
#ifdef COAL_HAS_QHULL
  void buildDoubleDescription();
#endif
 
  struct COAL_DLLAPI Neighbors {
    unsigned char count_;
    unsigned int* n_;
 
    unsigned char const& count() const { return count_; }
    unsigned int& operator[](int i) {
      assert(i < count_);
      return n_[i];
    }
    unsigned int const& operator[](int i) const {
      assert(i < count_);
      return n_[i];
    }
 
    bool operator==(const Neighbors& other) const {
      if (count_ != other.count_) return false;
 
      for (int i = 0; i < count_; ++i) {
        if (n_[i] != other.n_[i]) return false;
      }
 
      return true;
    }
 
    bool operator!=(const Neighbors& other) const { return !(*this == other); }
  };
 
  static constexpr size_t num_vertices_large_convex_threshold = 32;
 
  std::shared_ptr<std::vector<Vec3s>> points;
  unsigned int num_points;
 
  std::shared_ptr<std::vector<Vec3s>> normals;
  std::shared_ptr<std::vector<Scalar>> offsets;
  unsigned int num_normals_and_offsets;
 
  std::shared_ptr<std::vector<Neighbors>> neighbors;
 
  Vec3s center;
 
  struct SupportWarmStartPolytope {
    std::vector<Vec3s> points;
 
    std::vector<int> indices;
  };
 
  static constexpr size_t num_support_warm_starts = 14;
 
  SupportWarmStartPolytope support_warm_starts;
 
 protected:
  ConvexBase()
      : ShapeBase(),
        num_points(0),
        num_normals_and_offsets(0),
        center(Vec3s::Zero()) {}
 
  void initialize(std::shared_ptr<std::vector<Vec3s>> points_,
                  unsigned int num_points_);
 
  void set(std::shared_ptr<std::vector<Vec3s>> points_,
           unsigned int num_points_);
 
  ConvexBase(const ConvexBase& other);
 
#ifdef COAL_HAS_QHULL
  void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh);
#endif
 
  void buildSupportWarmStart();
 
  std::shared_ptr<std::vector<unsigned int>> nneighbors_;
 
 private:
  void computeCenter();
 
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const ConvexBase* other_ptr = dynamic_cast<const ConvexBase*>(&_other);
    if (other_ptr == nullptr) return false;
    const ConvexBase& other = *other_ptr;
 
    if (num_points != other.num_points) return false;
 
    if ((!(points.get()) && other.points.get()) ||
        (points.get() && !(other.points.get())))
      return false;
    if (points.get() && other.points.get()) {
      const std::vector<Vec3s>& points_ = *points;
      const std::vector<Vec3s>& other_points_ = *(other.points);
      for (unsigned int i = 0; i < num_points; ++i) {
        if (points_[i] != (other_points_)[i]) return false;
      }
    }
 
    if ((!(neighbors.get()) && other.neighbors.get()) ||
        (neighbors.get() && !(other.neighbors.get())))
      return false;
    if (neighbors.get() && other.neighbors.get()) {
      const std::vector<Neighbors>& neighbors_ = *neighbors;
      const std::vector<Neighbors>& other_neighbors_ = *(other.neighbors);
      for (unsigned int i = 0; i < num_points; ++i) {
        if (neighbors_[i] != other_neighbors_[i]) return false;
      }
    }
 
    if ((!(normals.get()) && other.normals.get()) ||
        (normals.get() && !(other.normals.get())))
      return false;
    if (normals.get() && other.normals.get()) {
      const std::vector<Vec3s>& normals_ = *normals;
      const std::vector<Vec3s>& other_normals_ = *(other.normals);
      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
        if (normals_[i] != other_normals_[i]) return false;
      }
    }
 
    if ((!(offsets.get()) && other.offsets.get()) ||
        (offsets.get() && !(other.offsets.get())))
      return false;
    if (offsets.get() && other.offsets.get()) {
      const std::vector<Scalar>& offsets_ = *offsets;
      const std::vector<Scalar>& other_offsets_ = *(other.offsets);
      for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
        if (offsets_[i] != other_offsets_[i]) return false;
      }
    }
 
    if (this->support_warm_starts.points.size() !=
            other.support_warm_starts.points.size() ||
        this->support_warm_starts.indices.size() !=
            other.support_warm_starts.indices.size()) {
      return false;
    }
 
    for (size_t i = 0; i < this->support_warm_starts.points.size(); ++i) {
      if (this->support_warm_starts.points[i] !=
              other.support_warm_starts.points[i] ||
          this->support_warm_starts.indices[i] !=
              other.support_warm_starts.indices[i]) {
        return false;
      }
    }
 
    return center == other.center &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
template <typename PolygonT>
class Convex;
 
class COAL_DLLAPI Halfspace : public ShapeBase {
 public:
  Halfspace(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
    unitNormalTest();
  }
 
  Halfspace(Scalar a, Scalar b, Scalar c, Scalar d_)
      : ShapeBase(), n(a, b, c), d(d_) {
    unitNormalTest();
  }
 
  Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
 
  Halfspace(const Halfspace& other)
      : ShapeBase(other), n(other.n), d(other.d) {}
 
  Halfspace& operator=(const Halfspace& other) {
    n = other.n;
    d = other.d;
    return *this;
  }
 
  virtual Halfspace* clone() const { return new Halfspace(*this); };
 
  Scalar signedDistance(const Vec3s& p) const {
    return n.dot(p) - (d + this->getSweptSphereRadius());
  }
 
  Scalar distance(const Vec3s& p) const {
    return std::abs(this->signedDistance(p));
  }
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
 
  Scalar minInflationValue() const {
    return std::numeric_limits<Scalar>::lowest();
  }
 
  std::pair<Halfspace, Transform3s> inflated(const Scalar value) const {
    if (value <= minInflationValue())
      COAL_THROW_PRETTY("value (" << value
                                  << ") is two small. It should be at least: "
                                  << minInflationValue(),
                        std::invalid_argument);
    return std::make_pair(Halfspace(n, d + value), Transform3s());
  }
 
  Vec3s n;
 
  Scalar d;
 
 protected:
  void unitNormalTest();
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
    if (other_ptr == nullptr) return false;
    const Halfspace& other = *other_ptr;
 
    return n == other.n && d == other.d &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
class COAL_DLLAPI Plane : public ShapeBase {
 public:
  Plane(const Vec3s& n_, Scalar d_) : ShapeBase(), n(n_), d(d_) {
    unitNormalTest();
  }
 
  Plane(Scalar a, Scalar b, Scalar c, Scalar d_)
      : ShapeBase(), n(a, b, c), d(d_) {
    unitNormalTest();
  }
 
  Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
 
  Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
 
  Plane& operator=(const Plane& other) {
    n = other.n;
    d = other.d;
    return *this;
  }
 
  virtual Plane* clone() const { return new Plane(*this); };
 
  Scalar signedDistance(const Vec3s& p) const {
    const Scalar dist = n.dot(p) - d;
    Scalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
    if (dist >= 0) {
      return signed_dist;
    }
    if (signed_dist >= 0) {
      return -signed_dist;
    }
    return signed_dist;
  }
 
  Scalar distance(const Vec3s& p) const {
    return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
  }
 
  void computeLocalAABB();
 
  NODE_TYPE getNodeType() const { return GEOM_PLANE; }
 
  Vec3s n;
 
  Scalar d;
 
 protected:
  void unitNormalTest();
 
 private:
  virtual bool isEqual(const CollisionGeometry& _other) const {
    const Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
    if (other_ptr == nullptr) return false;
    const Plane& other = *other_ptr;
 
    return n == other.n && d == other.d &&
           getSweptSphereRadius() == other.getSweptSphereRadius();
  }
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
}  // namespace coal
 
#endif