dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/collision/CollisionGroup.hpp

@@ -0,0 +1,546 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/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_COLLISIONGROUP_HPP_
+#define DART_COLLISION_COLLISIONGROUP_HPP_
+
+#include <dart/collision/CollisionOption.hpp>
+#include <dart/collision/CollisionResult.hpp>
+#include <dart/collision/DistanceOption.hpp>
+#include <dart/collision/DistanceResult.hpp>
+#include <dart/collision/RaycastOption.hpp>
+#include <dart/collision/RaycastResult.hpp>
+#include <dart/collision/SmartPointer.hpp>
+
+#include <dart/dynamics/SmartPointer.hpp>
+
+#include <dart/common/Observer.hpp>
+
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+namespace dart {
+namespace collision {
+
+class CollisionGroup
+{
+public:
+  /// Constructor
+  CollisionGroup(const CollisionDetectorPtr& collisionDetector);
+  // CollisionGroup also can be created from CollisionDetector::create()
+
+  /// Destructor
+  virtual ~CollisionGroup() = default;
+
+  /// Return collision detection engine associated with this CollisionGroup
+  CollisionDetectorPtr getCollisionDetector();
+
+  /// Return (const) collision detection engine associated with this
+  /// CollisionGroup
+  ConstCollisionDetectorPtr getCollisionDetector() const;
+
+  /// Add a ShapeFrame to this CollisionGroup
+  void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
+
+  /// Add ShapeFrames to this CollisionGroup
+  void addShapeFrames(
+      const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
+
+  /// Add a ShapeFrame, and also add ShapeFrames of other various objects.
+  ///
+  /// The other various objects can be any of ShapeFrame,
+  /// std::vector<ShapeFrame>, CollisionGroup, BodyNode, and Skeleton.
+  ///
+  /// Note that this function adds only the ShapeFrames of each object at the
+  /// moment of this function is called. The aftwerward changes of the objects
+  /// does not affect on this CollisionGroup.
+  template <typename... Others>
+  void addShapeFramesOf(
+      const dynamics::ShapeFrame* shapeFrame, const Others*... others);
+
+  /// Add ShapeFrames, and also add ShapeFrames of other various objects.
+  template <typename... Others>
+  void addShapeFramesOf(
+      const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
+      const Others*... others);
+
+  /// Add ShapeFrames of other CollisionGroup, and also add another ShapeFrames
+  /// of other various objects.
+  template <typename... Others>
+  void addShapeFramesOf(
+      const CollisionGroup* otherGroup, const Others*... others);
+
+  /// Add ShapeFrames of BodyNode, and also add another ShapeFrames of other
+  /// various objects.
+  template <typename... Others>
+  void addShapeFramesOf(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  /// Add ShapeFrames of MetaSkeleton, and also add another ShapeFrames of other
+  /// various objects.
+  template <typename... Others>
+  void addShapeFramesOf(
+      const dynamics::MetaSkeleton* skeleton, const Others*... others);
+
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template function template<typename...> addShapeFramesOf().
+  void addShapeFramesOf();
+
+  /// Add ShapeFrames of bodyNode, and also subscribe to the BodyNode so that
+  /// the results from this CollisionGroup automatically reflect any changes
+  /// that are made to bodyNode.
+  ///
+  /// This does likewise for the objects in ...others.
+  template <typename... Others>
+  void subscribeTo(
+      const dynamics::ConstBodyNodePtr& bodyNode, const Others&... others);
+
+  /// Add ShapeFrames of skeleton, and also subscribe to the Skeleton so that
+  /// the results from this CollisionGroup automatically reflect any changes
+  /// that are made to skeleton.
+  ///
+  /// This does likewise for the objects in ...others.
+  //
+  // TODO(MXG): Figure out how to determine a version number for MetaSkeletons
+  // so that this function can accept a ConstMetaSkeletonPtr instead of only a
+  // ConstSkeletonPtr.
+  template <typename... Others>
+  void subscribeTo(
+      const dynamics::ConstSkeletonPtr& skeleton, const Others&... others);
+
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template.
+  void subscribeTo();
+
+  /// Remove a ShapeFrame from this CollisionGroup. If this ShapeFrame was being
+  /// provided by any subscriptions, then calling this function will unsubscribe
+  /// from those subscriptions, because otherwise this ShapeFrame would simply
+  /// be put back into the CollisionGroup the next time the group gets updated.
+  void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
+
+  /// Remove ShapeFrames from this CollisionGroup
+  void removeShapeFrames(
+      const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
+
+  /// Remove a ShapeFrame, and also remove ShapeFrames of other various objects.
+  ///
+  /// The other various objects can be any of ShapeFrame,
+  /// std::vector<ShapeFrame>, CollisionGroup, BodyNode, and Skeleton.
+  ///
+  /// Note that this function removes only the ShapeFrames of each object at the
+  /// moment of this function is called. The aftwerward changes of the objects
+  /// does not affect on this CollisionGroup.
+  template <typename... Others>
+  void removeShapeFramesOf(
+      const dynamics::ShapeFrame* shapeFrame, const Others*... others);
+
+  /// Remove ShapeFrames, and also remove ShapeFrames of other various objects.
+  template <typename... Others>
+  void removeShapeFramesOf(
+      const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
+      const Others*... others);
+
+  /// Remove ShapeFrames of other CollisionGroup, and also remove another
+  /// ShapeFrames of other various objects.
+  ///
+  /// If this CollisionGroup is subscribed to otherGroup, then this function
+  /// will also unsubscribe from it.
+  template <typename... Others>
+  void removeShapeFramesOf(
+      const CollisionGroup* otherGroup, const Others*... others);
+
+  /// Remove ShapeFrames of BodyNode, and also remove another ShapeFrames of
+  /// other various objects.
+  ///
+  /// If this CollisionGroup is subscribed to bodyNode, then this function will
+  /// also unsubscribe from it.
+  template <typename... Others>
+  void removeShapeFramesOf(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  /// Remove ShapeFrames of MetaSkeleton, and also remove another ShapeFrames of
+  /// other various objects.
+  ///
+  /// If this CollisionGroup is subscribed to skeleton, then this function will
+  /// also unsubscribe from it.
+  template <typename... Others>
+  void removeShapeFramesOf(
+      const dynamics::MetaSkeleton* skeleton, const Others*... others);
+
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template function template<typename...> removeShapeFramesOf().
+  void removeShapeFramesOf();
+
+  /// Remove all the ShapeFrames in this CollisionGroup
+  void removeAllShapeFrames();
+
+  /// Unsubscribe from bodyNode. The ShapeFrames of the BodyNode will also be
+  /// removed if no other source is requesting them for this group.
+  template <typename... Others>
+  void unsubscribeFrom(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  /// Unsubscribe from skeleton. The ShapeFrames of the skeleton will also be
+  /// removed if no other source is requesting them for this group.
+  template <typename... Others>
+  void unsubscribeFrom(
+      const dynamics::Skeleton* skeleton, const Others*... others);
+
+  /// Do nothing. This function is for terminating the recursive variadic
+  /// template.
+  void unsubscribeFrom();
+
+  /// Check if this is subscribed to bodyNode and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::BodyNode* bodyNode, const Others*... others);
+
+  /// Check if this is subscribed to skeleton and the other sources
+  template <typename... Others>
+  bool isSubscribedTo(
+      const dynamics::Skeleton* skeleton, const Others*... others);
+
+  /// Return true. This function is for terminating the recursive variadic
+  /// template
+  bool isSubscribedTo();
+
+  /// Return true if this CollisionGroup contains shapeFrame
+  bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const;
+
+  /// Return number of ShapeFrames added to this CollisionGroup
+  std::size_t getNumShapeFrames() const;
+
+  /// Get the ShapeFrame corresponding to the given index
+  const dynamics::ShapeFrame* getShapeFrame(std::size_t index) const;
+
+  /// Perform collision check within this CollisionGroup.
+  bool collide(
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr);
+
+  /// Perform collision check with other CollisionGroup.
+  ///
+  /// Return false if the engine type of the other CollisionGroup is different
+  /// from this CollisionObject engine.
+  bool collide(
+      CollisionGroup* otherGroup,
+      const CollisionOption& option = CollisionOption(false, 1u, nullptr),
+      CollisionResult* result = nullptr);
+
+  /// Get the minimum signed distance between the Shape pairs in this
+  /// CollisionGroup.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  double distance(
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr);
+
+  /// Get the minimum signed distance between the Shape pairs where a pair
+  /// consist of two shapes from each groups (one from this CollisionGroup and
+  /// one from otherGroup).
+  ///
+  /// Note that the distance between shapes within the same CollisionGroup
+  /// are not accounted.
+  ///
+  /// The detailed results are stored in the given DistanceResult if provided.
+  ///
+  /// The results can be different by DistanceOption. By default, non-negative
+  /// minimum distance (distance >= 0) is returned for all the shape pairs
+  /// without computing nearest points.
+  double distance(
+      CollisionGroup* otherGroup,
+      const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
+      DistanceResult* result = nullptr);
+
+  /// Performs raycast to this collision group.
+  ///
+  /// \param[in] from The start point of the ray in world coordinates.
+  /// \param[in] to The end point of the ray in world coordinates.
+  /// \param[in] option The raycast option.
+  /// \param[in] result The raycast result.
+  /// \return True if the ray hit an collision object.
+  bool raycast(
+      const Eigen::Vector3d& from,
+      const Eigen::Vector3d& to,
+      const RaycastOption& option = RaycastOption(),
+      RaycastResult* result = nullptr);
+
+  /// Set whether this CollisionGroup will automatically check for updates.
+  void setAutomaticUpdate(bool automatic = true);
+
+  /// Get whether this CollisionGroup is set to automatically check for updates.
+  bool getAutomaticUpdate() const;
+
+  /// Check whether this CollisionGroup's subscriptions or any of its objects
+  /// need an update, and then update them if they do.
+  ///
+  /// If automatic updating is turned on, then this will be called each time
+  /// collide() is.
+  ///
+  /// This can be called manually in order to control when collision objects get
+  /// created or destroyed. Those procedures can be time-consuming, so it may be
+  /// useful to control when it occurs.
+  void update();
+
+  /// Remove any ShapeFrames that have been deleted. This will be done
+  /// automatically for ShapeFrames that belong to subscriptions.
+  ///
+  /// Returns true if one or more ShapeFrames were removed; otherwise returns
+  /// false.
+  void removeDeletedShapeFrames();
+
+protected:
+  /// Update engine data. This function should be called before the collision
+  /// detection is performed by the engine in most cases.
+  void updateEngineData();
+
+  /// Initialize the collision detection engine data such as broadphase
+  /// algorithm. This function will be called whenever ShapeFrame is either
+  /// added to or removed from this CollisionGroup.
+  virtual void initializeEngineData() = 0;
+
+  /// Add CollisionObject to the collision detection engine
+  virtual void addCollisionObjectToEngine(CollisionObject* object) = 0;
+
+  /// Add CollisionObjects to the collision detection engine
+  virtual void addCollisionObjectsToEngine(
+      const std::vector<CollisionObject*>& collObjects)
+      = 0;
+
+  /// Remove CollisionObject from the collision detection engine
+  virtual void removeCollisionObjectFromEngine(CollisionObject* object) = 0;
+
+  /// Remove all the CollisionObjects from the collision detection engine
+  virtual void removeAllCollisionObjectsFromEngine() = 0;
+
+  /// Update the collision detection engine data such as broadphase algorithm.
+  /// This function will be called ahead of every collision checking.
+  virtual void updateCollisionGroupEngineData() = 0;
+
+protected:
+  /// Collision detector
+  CollisionDetectorPtr mCollisionDetector;
+  // CollisionGroup shares the ownership of CollisionDetector with other
+  // CollisionGroups created from the same CollisionDetector so that the
+  // CollisionDetector doesn't get destroyed as long as at least one
+  // CollisionGroup is alive.
+
+  /// Information on a collision object belonging to this group. This info
+  /// enables us to keep track of when a ShapeFrame has changed and therefore
+  /// when the object needs to be updated. It also remembers what sources
+  /// instructed this object to belong to this group.
+  struct ObjectInfo final
+  {
+    /// The ShapeFrame for this object
+    const dynamics::ShapeFrame* mFrame;
+
+    /// The CollisionObject that was generated by the CollisionDetector
+    CollisionObjectPtr mObject;
+
+    /// The ID of that last known shape that was held by the shape frame
+    std::size_t mLastKnownShapeID;
+
+    /// The last known version of the last known shape that was held by the
+    /// shape frame
+    std::size_t mLastKnownVersion;
+
+    /// The set of all sources that indicate that this object should be in this
+    /// group. In the current implementation, this may consist of:
+    /// user (nullptr), Skeleton subscription, and/or BodyNode subscription.
+    ///
+    /// When all sources are cleared out (via unsubscribing), this object will
+    /// be removed from this group.
+    std::unordered_set<const void*> mSources;
+  };
+
+  using ObjectInfoList = std::vector<std::unique_ptr<ObjectInfo>>;
+
+  /// Information about ShapeFrames and CollisionObjects that have been added to
+  /// this CollisionGroup.
+  ObjectInfoList mObjectInfoList;
+  // CollisionGroup also shares the ownership of CollisionObjects across other
+  // CollisionGroups for the same reason with above.
+  //
+  // Dev note: This was supposed to be std::map rather than std::vector for
+  // better search performance. The reason we use std::vector is to get
+  // deterministic contact results regardless of the order of CollisionObjects
+  // in this container for FCLCollisionDetector.
+  //
+  // fcl's collision result is dependent on the order of objects in the broad
+  // phase classes. If we use std::map, the orders of element between the
+  // original and copy are not guranteed to be the same as we copy std::map
+  // (e.g., by world cloning).
+
+private:
+  /// This class watches when ShapeFrames get deleted so that they can be safely
+  /// removes from the CollisionGroup. We cannot have a weak_ptr to a ShapeFrame
+  /// because some are managed by std::shared_ptr while others are managed by
+  /// NodePtr.
+  ///
+  /// If we don't carefully track when each ShapeFrame gets destructed, then we
+  /// can land in a situation where a ShapeFrame gets removed from a BodyNode,
+  /// deallocated, and then a new ShapeFrame is allocated in the memory address
+  /// of the old one. This can lead to invalid memory accesses if we neglect to
+  /// correctly clean up our references to deleted ShapeFrames.
+  class ShapeFrameObserver final : public common::Observer
+  {
+  public:
+    /// Add a shape frame to this observer
+    void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
+
+    /// Remove a shape frame from this observer
+    void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
+
+    /// Remove all shape frames from this observer
+    void removeAllShapeFrames();
+
+    /// Whenever an observed shape frame gets deleted, its pointer will be added
+    /// to this set. The next time the collision group updates, it will check
+    /// this set and wipe away any references to the pointers in this set.
+    std::unordered_set<const dynamics::ShapeFrame*> mDeletedFrames;
+
+  protected:
+    /// This will be called each time an observed shape frame is deleted.
+    void handleDestructionNotification(const common::Subject* subject) override;
+
+  private:
+    /// A map from a subject pointer to its corresponding ShapeFrame pointer.
+    /// This needs to be stored because by the time a Subject is being
+    /// destructed, it can no longer be cast back to its ShapeFrame.
+    std::unordered_map<const common::Subject*, const dynamics::ShapeFrame*>
+        mMap;
+  };
+
+  /// Implementation of addShapeFrame. The source argument tells us whether this
+  /// ShapeFrame is being requested explicitly by the user or implicitly through
+  /// a BodyNode, Skeleton, or other CollisionGroup.
+  ObjectInfo* addShapeFrameImpl(
+      const dynamics::ShapeFrame* shapeFrame, const void* source);
+
+  /// Internal version of removeShapeFrame. This will only remove the ShapeFrame
+  /// if it is unsubscribed from all sources.
+  void removeShapeFrameInternal(
+      const dynamics::ShapeFrame* shapeFrame, const void* source);
+
+  /// Set this to true to have this CollisionGroup check for updates
+  /// automatically. Default is true.
+  bool mUpdateAutomatically;
+
+  /// \private This struct is used to store sources of ShapeFrames that the
+  /// CollisionGroup is subscribed to, alongside the last version number of that
+  /// source, as known by this CollisionGroup.
+  template <typename Source, typename Child = void>
+  struct CollisionSource
+  {
+    /// The source of ShapeFrames
+    Source mSource;
+
+    /// The last known version of that source
+    std::size_t mLastKnownVersion;
+
+    /// The set of objects that pertain to this source
+    std::unordered_map<const dynamics::ShapeFrame*, ObjectInfo*> mObjects;
+
+    /// This is information pertaining to a child of the source. In the current
+    /// implementation, this only gets used by SkeletonSources.
+    struct ChildInfo
+    {
+      /// Last known version of this child
+      std::size_t mLastKnownVersion;
+
+      /// Last known set of frames attached to this child
+      std::unordered_set<const dynamics::ShapeFrame*> mFrames;
+
+      /// A constructor that simply accepts a last known version number. Shape
+      /// frames can be added later.
+      explicit ChildInfo(const std::size_t version) : mLastKnownVersion(version)
+      {
+        // Do nothing
+      }
+    };
+
+    /// The last known versions of the children related to this source. This is
+    /// used by SkeletonSources to keep track of their child BodyNode versions.
+    std::unordered_map<const Child*, ChildInfo> mChildren;
+
+    /// Constructor
+    CollisionSource(const Source& source, std::size_t lastKnownVersion)
+      : mSource(source), mLastKnownVersion(lastKnownVersion)
+    {
+      // Do nothing
+    }
+  };
+
+  // Convenient typedefs for Skeleton sources
+  using SkeletonSource
+      = CollisionSource<dynamics::WeakConstMetaSkeletonPtr, dynamics::BodyNode>;
+  using SkeletonSources
+      = std::unordered_map<const dynamics::MetaSkeleton*, SkeletonSource>;
+
+  // Convenient typedefs for BodyNode sources
+  using BodyNodeSource = CollisionSource<dynamics::WeakConstBodyNodePtr>;
+  using BodyNodeSources
+      = std::unordered_map<const dynamics::BodyNode*, BodyNodeSource>;
+
+  /// Internal function called to update a Skeleton source
+  /// \returns true if an update was performed
+  bool updateSkeletonSource(SkeletonSources::value_type& entry);
+
+  /// Internal function called to update a BodyNode source
+  /// \returns true if an update was performed
+  bool updateBodyNodeSource(BodyNodeSources::value_type& entry);
+
+  /// Internal function called to update a ShapeFrame
+  /// \returns true if an update was performed
+  bool updateShapeFrame(ObjectInfo* object);
+
+  /// Skeleton sources that this group is subscribed to
+  SkeletonSources mSkeletonSources;
+
+  /// BodyNode sources that this group is susbscribed to
+  BodyNodeSources mBodyNodeSources;
+
+  /// The object that observes the Shape Frames that this group cares about
+  ShapeFrameObserver mObserver;
+};
+
+} // namespace collision
+} // namespace dart
+
+#include <dart/collision/detail/CollisionGroup.hpp>
+
+#endif // DART_COLLISION_COLLISIONGROUP_HPP_

include/dart/collision/CollisionObject.hpp

@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/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_COLLISIONOBJECT_HPP_
+#define DART_COLLISION_COLLISIONOBJECT_HPP_
+
+#include <dart/collision/SmartPointer.hpp>
+
+#include <dart/dynamics/SmartPointer.hpp>
+
+#include <Eigen/Dense>
+
+namespace dart {
+namespace collision {
+
+class CollisionObject
+{
+public:
+  friend class CollisionGroup;
+
+  /// Destructor
+  virtual ~CollisionObject() = default;
+
+  /// Return collision detection engine associated with this CollisionObject
+  CollisionDetector* getCollisionDetector();
+
+  /// Return collision detection engine associated with this CollisionObject
+  const CollisionDetector* getCollisionDetector() const;
+
+  /// Return the associated ShapeFrame
+  const dynamics::ShapeFrame* getShapeFrame() const;
+
+  /// Return the associated Shape
+  dynamics::ConstShapePtr getShape() const;
+
+  /// Return the transformation of this CollisionObject in world coordinates
+  const Eigen::Isometry3d& getTransform() const;
+
+protected:
+  /// Contructor
+  CollisionObject(
+      CollisionDetector* collisionDetector,
+      const dynamics::ShapeFrame* shapeFrame);
+
+  /// Update the collision object of the collision detection engine. This
+  /// function will be called ahead of every collision checking by
+  /// CollisionGroup.
+  virtual void updateEngineData() = 0;
+
+protected:
+  /// Collision detector
+  CollisionDetector* mCollisionDetector;
+
+  /// ShapeFrame
+  const dynamics::ShapeFrame* mShapeFrame;
+};
+
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_COLLISIONOBJECT_HPP_