World.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 code incorporates portions of Open Dynamics Engine
 *     (Copyright (c) 2001-2004, Russell L. Smith. All rights
 *     reserved.) and portions of FCL (Copyright (c) 2011, Willow
 *     Garage, Inc. All rights reserved.), which were released under
 *     the same BSD license as below
 *
 *   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_SIMULATION_WORLD_HPP_
#define DART_SIMULATION_WORLD_HPP_

#include <set>
#include <string>
#include <vector>

#include <Eigen/Dense>

#include "dart/common/NameManager.hpp"
#include "dart/common/SmartPointer.hpp"
#include "dart/common/Subject.hpp"
#include "dart/common/Timer.hpp"
#include "dart/dynamics/CollisionOption.hpp"
#include "dart/dynamics/SimpleFrame.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/dynamics/SmartPointer.hpp"
#include "dart/simulation/Recording.hpp"
#include "dart/simulation/SmartPointer.hpp"

namespace dart {

namespace math {
class Integrator;
} // namespace math

namespace dynamics {
class Skeleton;
} // namespace dynamics

namespace dynamics {
class ConstraintSolver;
} // namespace dynamics

namespace collision {
class CollisionResult;
} // namespace collision

namespace simulation {

DART_COMMON_DECLARE_SHARED_WEAK(World)

DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
class World : public virtual common::Subject
{
public:
  using NameChangedSignal = common::Signal<void(
      const std::string& _oldName, const std::string& _newName)>;

  template <typename... Args>
  static WorldPtr create(Args&&... args);

  //--------------------------------------------------------------------------
  // Constructor and Destructor
  //--------------------------------------------------------------------------

  static std::shared_ptr<World> create(const std::string& name = "world");

  World(const std::string& _name = "world");

  virtual ~World();

  std::shared_ptr<World> clone() const;

  //--------------------------------------------------------------------------
  // Properties
  //--------------------------------------------------------------------------

  const std::string& setName(const std::string& _newName);

  const std::string& getName() const;

  void setGravity(const Eigen::Vector3d& _gravity);

  const Eigen::Vector3d& getGravity() const;

  void setTimeStep(double _timeStep);

  double getTimeStep() const;

  //--------------------------------------------------------------------------
  // Structural Properties
  //--------------------------------------------------------------------------

  dynamics::SkeletonPtr getSkeleton(std::size_t _index) const;

  dynamics::SkeletonPtr getSkeleton(const std::string& _name) const;

  std::size_t getNumSkeletons() const;

  std::string addSkeleton(const dynamics::SkeletonPtr& _skeleton);

  void removeSkeleton(const dynamics::SkeletonPtr& _skeleton);

  std::set<dynamics::SkeletonPtr> removeAllSkeletons();

  bool hasSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const;

  bool hasSkeleton(const std::string& skeletonName) const;

  int getIndex(int _index) const;

  dynamics::SimpleFramePtr getSimpleFrame(std::size_t _index) const;

  dynamics::SimpleFramePtr getSimpleFrame(const std::string& _name) const;

  std::size_t getNumSimpleFrames() const;

  std::string addSimpleFrame(const dynamics::SimpleFramePtr& _frame);

  void removeSimpleFrame(const dynamics::SimpleFramePtr& _frame);

  std::set<dynamics::SimpleFramePtr> removeAllSimpleFrames();

  //--------------------------------------------------------------------------
  // Collision checking
  //--------------------------------------------------------------------------

  DART_DEPRECATED(6.0)
  bool checkCollision(bool checkAllCollisions);

  bool checkCollision(
      const collision::CollisionOption& option
      = collision::CollisionOption(false, 1u, nullptr),
      collision::CollisionResult* result = nullptr);

  const collision::CollisionResult& getLastCollisionResult() const;

  //--------------------------------------------------------------------------
  // Simulation
  //--------------------------------------------------------------------------

  void reset();

  void step(bool _resetCommand = true);

  void setTime(double _time);

  double getTime() const;

  int getSimFrames() const;

  //--------------------------------------------------------------------------
  // Constraint
  //--------------------------------------------------------------------------

  void setConstraintSolver(dynamics::UniqueConstraintSolverPtr solver);

  dynamics::ConstraintSolver* getConstraintSolver();

  const dynamics::ConstraintSolver* getConstraintSolver() const;

  void bake();

  Recording* getRecording();

protected:
  void handleSkeletonNameChange(
      const dynamics::ConstMetaSkeletonPtr& _skeleton);

  void handleSimpleFrameNameChange(const dynamics::Entity* _entity);

  std::string mName;

  std::vector<dynamics::SkeletonPtr> mSkeletons;

  std::map<dynamics::ConstMetaSkeletonPtr, dynamics::SkeletonPtr>
      mMapForSkeletons;

  std::vector<common::Connection> mNameConnectionsForSkeletons;

  dart::common::NameManager<dynamics::SkeletonPtr> mNameMgrForSkeletons;

  std::vector<dynamics::SimpleFramePtr> mSimpleFrames;

  std::vector<common::Connection> mNameConnectionsForSimpleFrames;

  std::map<const dynamics::SimpleFrame*, dynamics::SimpleFramePtr>
      mSimpleFrameToShared;

  dart::common::NameManager<dynamics::SimpleFramePtr> mNameMgrForSimpleFrames;

  std::vector<int> mIndices;

  Eigen::Vector3d mGravity;

  double mTimeStep;

  double mTime;

  int mFrame;

  std::unique_ptr<dynamics::ConstraintSolver> mConstraintSolver;

  Recording* mRecording;

  //--------------------------------------------------------------------------
  // Signals
  //--------------------------------------------------------------------------
  NameChangedSignal mNameChangedSignal;

public:
  //--------------------------------------------------------------------------
  // Slot registers
  //--------------------------------------------------------------------------
  common::SlotRegister<NameChangedSignal> onNameChanged;
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END

} // namespace simulation
} // namespace dart

#include "dart/simulation/detail/World-impl.hpp"

#endif // DART_SIMULATION_WORLD_HPP_