CollisionDetector.hpp

/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * 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.
 */

#ifndef DART_COLLISION_COLLISIONDETECTOR_HPP_
#define DART_COLLISION_COLLISIONDETECTOR_HPP_

#include <map>
#include <vector>

#include <Eigen/Dense>

#include "dart/common/Factory.hpp"
#include "dart/dynamics/CollisionOption.hpp"
#include "dart/dynamics/CollisionResult.hpp"
#include "dart/dynamics/Contact.hpp"
#include "dart/dynamics/DistanceOption.hpp"
#include "dart/dynamics/DistanceResult.hpp"
#include "dart/dynamics/RaycastOption.hpp"
#include "dart/dynamics/RaycastResult.hpp"
#include "dart/dynamics/SmartPointer.hpp"

namespace dart {
namespace collision {

class CollisionObject;

class CollisionDetector : public std::enable_shared_from_this<CollisionDetector>
{
public:
  friend class CollisionObject;
  friend class CollisionGroup;

  using Factory = common::Factory<
      std::string,
      CollisionDetector,
      std::shared_ptr<CollisionDetector>>;

  using SingletonFactory = common::Singleton<Factory>;

  template <typename Derived>
  using Registrar = common::FactoryRegistrar<
      std::string,
      CollisionDetector,
      Derived,
      std::shared_ptr<CollisionDetector>>;

  static Factory* getFactory();

  virtual ~CollisionDetector() = default;

  virtual std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects()
      const = 0;

  virtual const std::string& getType() const = 0;

  virtual std::unique_ptr<CollisionGroup> createCollisionGroup() = 0;

  std::shared_ptr<CollisionGroup> createCollisionGroupAsSharedPtr();

  template <typename... Args>
  std::unique_ptr<CollisionGroup> createCollisionGroup(const Args&... args);

  template <typename... Args>
  std::shared_ptr<CollisionGroup> createCollisionGroupAsSharedPtr(
      const Args&... args);

  virtual bool collide(
      CollisionGroup* group,
      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
      CollisionResult* result = nullptr)
      = 0;

  virtual bool collide(
      CollisionGroup* group1,
      CollisionGroup* group2,
      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
      CollisionResult* result = nullptr)
      = 0;

  virtual double distance(
      CollisionGroup* group,
      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
      DistanceResult* result = nullptr)
      = 0;

  virtual double distance(
      CollisionGroup* group1,
      CollisionGroup* group2,
      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
      DistanceResult* result = nullptr)
      = 0;

  virtual bool raycast(
      CollisionGroup* group,
      const Eigen::Vector3d& from,
      const Eigen::Vector3d& to,
      const RaycastOption& option = RaycastOption(),
      RaycastResult* result = nullptr);

protected:
  class CollisionObjectManager;
  class ManagerForUnsharableCollisionObjects;
  class ManagerForSharableCollisionObjects;

  CollisionDetector() = default;

  std::shared_ptr<CollisionObject> claimCollisionObject(
      const dynamics::ShapeFrame* shapeFrame);

  virtual std::unique_ptr<CollisionObject> createCollisionObject(
      const dynamics::ShapeFrame* shapeFrame)
      = 0;

  virtual void refreshCollisionObject(CollisionObject* object) = 0;

  virtual void notifyCollisionObjectDestroying(CollisionObject* object);

protected:
  std::unique_ptr<CollisionObjectManager> mCollisionObjectManager;
};

//==============================================================================
class CollisionDetector::CollisionObjectManager
{
public:
  CollisionObjectManager(CollisionDetector* cd);

  virtual std::shared_ptr<CollisionObject> claimCollisionObject(
      const dynamics::ShapeFrame* shapeFrame)
      = 0;

  CollisionDetector* getCollisionDetector();

  virtual ~CollisionObjectManager() = default;

protected:
  CollisionDetector* mCollisionDetector;
};

//==============================================================================
class CollisionDetector::ManagerForUnsharableCollisionObjects final
  : public CollisionDetector::CollisionObjectManager
{
public:
  ManagerForUnsharableCollisionObjects(CollisionDetector* cd);

  // Documentation inherited
  std::shared_ptr<CollisionObject> claimCollisionObject(
      const dynamics::ShapeFrame* shapeFrame);

private:
  struct CollisionObjectDeleter final
  {
    ManagerForUnsharableCollisionObjects* mCollisionObjectManager;

    CollisionObjectDeleter(ManagerForUnsharableCollisionObjects* mgr);

    void operator()(CollisionObject* object) const;
  };

  const CollisionObjectDeleter mCollisionObjectDeleter;
};

//==============================================================================
class CollisionDetector::ManagerForSharableCollisionObjects final
  : public CollisionDetector::CollisionObjectManager
{
public:
  ManagerForSharableCollisionObjects(CollisionDetector* cd);

  virtual ~ManagerForSharableCollisionObjects();

  // Documentation inherited
  std::shared_ptr<CollisionObject> claimCollisionObject(
      const dynamics::ShapeFrame* shapeFrame);

private:
  struct CollisionObjectDeleter final
  {
    ManagerForSharableCollisionObjects* mCollisionObjectManager;

    CollisionObjectDeleter(ManagerForSharableCollisionObjects* mgr);

    void operator()(CollisionObject* object) const;
  };

  const CollisionObjectDeleter mCollisionObjectDeleter;

  using CollisionObjectMap
      = std::map<const dynamics::ShapeFrame*, std::weak_ptr<CollisionObject>>;

  CollisionObjectMap mCollisionObjectMap;
};

} // namespace collision
} // namespace dart

#include "dart/dynamics/detail/CollisionDetector.hpp"

#endif // DART_COLLISION_COLLISIONDETECTOR_HPP_