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(CoalScalar radius) {
 if (radius < 0) {
 COAL_THROW_PRETTY("Swept-sphere radius must be positive.",
 std::invalid_argument);
 }
 this->m_swept_sphere_radius = radius;
 }

 CoalScalar getSweptSphereRadius() const {
 return this->m_swept_sphere_radius;
 }

 protected:
 CoalScalar 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 CoalScalar 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());
 // }
 //
 // CoalScalar minInflationValue() const
 // {
 // return (std::numeric_limits<CoalScalar>::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(CoalScalar x, CoalScalar y, CoalScalar 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; }

 CoalScalar computeVolume() const { return 8 * halfSide.prod(); }

 Matrix3s computeMomentofInertia() const {
 CoalScalar V = computeVolume();
 Vec3s s(halfSide.cwiseAbs2() * V);
 return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
 }

 CoalScalar minInflationValue() const { return -halfSide.minCoeff(); }

 std::pair<Box, Transform3s> inflated(const CoalScalar 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(CoalScalar radius_) : ShapeBase(), radius(radius_) {}

 Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}

 virtual Sphere* clone() const { return new Sphere(*this); };

 CoalScalar radius;

 void computeLocalAABB();

 NODE_TYPE getNodeType() const { return GEOM_SPHERE; }

 Matrix3s computeMomentofInertia() const {
 CoalScalar I = 0.4 * radius * radius * computeVolume();
 return I * Matrix3s::Identity();
 }

 CoalScalar computeVolume() const {
 return 4 * boost::math::constants::pi<CoalScalar>() * radius * radius *
 radius / 3;
 }

 CoalScalar minInflationValue() const { return -radius; }

 std::pair<Sphere, Transform3s> inflated(const CoalScalar 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(CoalScalar rx, CoalScalar ry, CoalScalar 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 {
 CoalScalar V = computeVolume();
 CoalScalar a2 = V * radii[0] * radii[0];
 CoalScalar b2 = V * radii[1] * radii[1];
 CoalScalar c2 = V * radii[2] * radii[2];
 return (Matrix3s() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
 0.2 * (a2 + b2))
 .finished();
 }

 CoalScalar computeVolume() const {
 return 4 * boost::math::constants::pi<CoalScalar>() * radii[0] * radii[1] *
 radii[2] / 3;
 }

 CoalScalar minInflationValue() const { return -radii.minCoeff(); }

 std::pair<Ellipsoid, Transform3s> inflated(const CoalScalar 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(CoalScalar radius_, CoalScalar 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); };

 CoalScalar radius;

 CoalScalar halfLength;

 void computeLocalAABB();

 NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }

 CoalScalar computeVolume() const {
 return boost::math::constants::pi<CoalScalar>() * radius * radius *
 ((halfLength * 2) + radius * 4 / 3.0);
 }

 Matrix3s computeMomentofInertia() const {
 CoalScalar v_cyl = radius * radius * (halfLength * 2) *
 boost::math::constants::pi<CoalScalar>();
 CoalScalar v_sph = radius * radius * radius *
 boost::math::constants::pi<CoalScalar>() * 4 / 3.0;

 CoalScalar h2 = halfLength * halfLength;
 CoalScalar r2 = radius * radius;
 CoalScalar ix = v_cyl * (h2 / 3. + r2 / 4.) +
 v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
 CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;

 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
 }

 CoalScalar minInflationValue() const { return -radius; }

 std::pair<Capsule, Transform3s> inflated(const CoalScalar 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(CoalScalar radius_, CoalScalar 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); };

 CoalScalar radius;

 CoalScalar halfLength;

 void computeLocalAABB();

 NODE_TYPE getNodeType() const { return GEOM_CONE; }

 CoalScalar computeVolume() const {
 return boost::math::constants::pi<CoalScalar>() * radius * radius *
 (halfLength * 2) / 3;
 }

 Matrix3s computeMomentofInertia() const {
 CoalScalar V = computeVolume();
 CoalScalar ix =
 V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
 CoalScalar iz = 0.3 * V * radius * radius;

 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
 }

 Vec3s computeCOM() const { return Vec3s(0, 0, -0.5 * halfLength); }

 CoalScalar minInflationValue() const {
 return -(std::min)(radius, halfLength);
 }

 std::pair<Cone, Transform3s> inflated(const CoalScalar 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 CoalScalar tan_alpha = 2 * halfLength / radius;
 const CoalScalar sin_alpha =
 tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
 const CoalScalar top_inflation = value / sin_alpha;
 const CoalScalar bottom_inflation = value;

 const CoalScalar new_lz = 2 * halfLength + top_inflation + bottom_inflation;
 const CoalScalar new_cz = (top_inflation + bottom_inflation) / 2.;
 const CoalScalar 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(CoalScalar radius_, CoalScalar 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); };

 CoalScalar radius;

 CoalScalar halfLength;

 void computeLocalAABB();

 NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }

 CoalScalar computeVolume() const {
 return boost::math::constants::pi<CoalScalar>() * radius * radius *
 (halfLength * 2);
 }

 Matrix3s computeMomentofInertia() const {
 CoalScalar V = computeVolume();
 CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
 CoalScalar iz = V * radius * radius / 2;
 return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
 }

 CoalScalar minInflationValue() const {
 return -(std::min)(radius, halfLength);
 }

 std::pair<Cylinder, Transform3s> inflated(const CoalScalar 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<double>> 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<double>& offsets_ = *offsets;
 const std::vector<double>& 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_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
 unitNormalTest();
 }

 Halfspace(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar 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); };

 CoalScalar signedDistance(const Vec3s& p) const {
 return n.dot(p) - (d + this->getSweptSphereRadius());
 }

 CoalScalar distance(const Vec3s& p) const {
 return std::abs(this->signedDistance(p));
 }

 void computeLocalAABB();

 NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }

 CoalScalar minInflationValue() const {
 return std::numeric_limits<CoalScalar>::lowest();
 }

 std::pair<Halfspace, Transform3s> inflated(const CoalScalar 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;

 CoalScalar 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_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
 unitNormalTest();
 }

 Plane(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar 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); };

 CoalScalar signedDistance(const Vec3s& p) const {
 const CoalScalar dist = n.dot(p) - d;
 CoalScalar 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;
 }

 CoalScalar 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;

 CoalScalar 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