.. _program_listing_file_include_coal_collision_object.h:

Program Listing for File collision_object.h
===========================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_coal_collision_object.h>` (``include/coal/collision_object.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * 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_COLLISION_OBJECT_BASE_H
   #define COAL_COLLISION_OBJECT_BASE_H
   
   #include <limits>
   #include <typeinfo>
   
   #include "coal/deprecated.hh"
   #include "coal/fwd.hh"
   #include "coal/BV/AABB.h"
   #include "coal/math/transform.h"
   
   namespace coal {
   
   enum OBJECT_TYPE {
     OT_UNKNOWN,
     OT_BVH,
     OT_GEOM,
     OT_OCTREE,
     OT_HFIELD,
     OT_COUNT
   };
   
   enum NODE_TYPE {
     BV_UNKNOWN,
     BV_AABB,
     BV_OBB,
     BV_RSS,
     BV_kIOS,
     BV_OBBRSS,
     BV_KDOP16,
     BV_KDOP18,
     BV_KDOP24,
     GEOM_BOX,
     GEOM_SPHERE,
     GEOM_CAPSULE,
     GEOM_CONE,
     GEOM_CYLINDER,
     GEOM_CONVEX,
     GEOM_PLANE,
     GEOM_HALFSPACE,
     GEOM_TRIANGLE,
     GEOM_OCTREE,
     GEOM_ELLIPSOID,
     HF_AABB,
     HF_OBBRSS,
     NODE_COUNT
   };
   
   
   class COAL_DLLAPI CollisionGeometry {
    public:
     CollisionGeometry()
         : aabb_center(Vec3s::Constant((std::numeric_limits<Scalar>::max)())),
           aabb_radius(-1),
           cost_density(1),
           threshold_occupied(1),
           threshold_free(0) {}
   
     CollisionGeometry(const CollisionGeometry& other) = default;
   
     virtual ~CollisionGeometry() {}
   
     virtual CollisionGeometry* clone() const = 0;
   
     bool operator==(const CollisionGeometry& other) const {
       return cost_density == other.cost_density &&
              threshold_occupied == other.threshold_occupied &&
              threshold_free == other.threshold_free &&
              aabb_center == other.aabb_center &&
              aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
              isEqual(other);
     }
   
     bool operator!=(const CollisionGeometry& other) const {
       return isNotEqual(other);
     }
   
     virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
   
     virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
   
     virtual void computeLocalAABB() = 0;
   
     void* getUserData() const { return user_data; }
   
     void setUserData(void* data) { user_data = data; }
   
     inline bool isOccupied() const { return cost_density >= threshold_occupied; }
   
     inline bool isFree() const { return cost_density <= threshold_free; }
   
     bool isUncertain() const;
   
     Vec3s aabb_center;
   
     Scalar aabb_radius;
   
     AABB aabb_local;
   
     void* user_data;
   
     Scalar cost_density;
   
     Scalar threshold_occupied;
   
     Scalar threshold_free;
   
     virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
   
     virtual Matrix3s computeMomentofInertia() const {
       return Matrix3s::Constant(NAN);
     }
   
     virtual Scalar computeVolume() const { return 0; }
   
     virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
       Matrix3s C = computeMomentofInertia();
       Vec3s com = computeCOM();
       Scalar V = computeVolume();
   
       return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
               C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
               C(1, 0) + V * com[1] * com[0],
               C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
               C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
               C(2, 1) + V * com[2] * com[1],
               C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
           .finished();
     }
   
    private:
     virtual bool isEqual(const CollisionGeometry& other) const = 0;
   
     virtual bool isNotEqual(const CollisionGeometry& other) const {
       return !(*this == other);
     }
   
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };
   
   class COAL_DLLAPI CollisionObject {
    public:
     CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
                     bool compute_local_aabb = true)
         : cgeom(cgeom_), user_data(nullptr) {
       init(compute_local_aabb);
     }
   
     CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
                     const Transform3s& tf, bool compute_local_aabb = true)
         : cgeom(cgeom_), t(tf), user_data(nullptr) {
       init(compute_local_aabb);
     }
   
     CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
                     const Matrix3s& R, const Vec3s& T,
                     bool compute_local_aabb = true)
         : cgeom(cgeom_), t(R, T), user_data(nullptr) {
       init(compute_local_aabb);
     }
   
     bool operator==(const CollisionObject& other) const {
       return cgeom == other.cgeom && t == other.t && user_data == other.user_data;
     }
   
     bool operator!=(const CollisionObject& other) const {
       return !(*this == other);
     }
   
     ~CollisionObject() {}
   
     OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
   
     NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
   
     const AABB& getAABB() const { return aabb; }
   
     AABB& getAABB() { return aabb; }
   
     void computeAABB() {
       if (t.getRotation().isIdentity()) {
         aabb = translate(cgeom->aabb_local, t.getTranslation());
       } else {
         aabb.min_ = aabb.max_ = t.getTranslation();
   
         Vec3s min_world, max_world;
         for (int k = 0; k < 3; ++k) {
           min_world.array() = t.getRotation().row(k).array() *
                               cgeom->aabb_local.min_.transpose().array();
           max_world.array() = t.getRotation().row(k).array() *
                               cgeom->aabb_local.max_.transpose().array();
   
           aabb.min_[k] += (min_world.array().min)(max_world.array()).sum();
           aabb.max_[k] += (min_world.array().max)(max_world.array()).sum();
         }
       }
     }
   
     void* getUserData() const { return user_data; }
   
     void setUserData(void* data) { user_data = data; }
   
     inline const Vec3s& getTranslation() const { return t.getTranslation(); }
   
     inline const Matrix3s& getRotation() const { return t.getRotation(); }
   
     inline const Transform3s& getTransform() const { return t; }
   
     void setRotation(const Matrix3s& R) { t.setRotation(R); }
   
     void setTranslation(const Vec3s& T) { t.setTranslation(T); }
   
     void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
   
     void setTransform(const Transform3s& tf) { t = tf; }
   
     bool isIdentityTransform() const { return t.isIdentity(); }
   
     void setIdentityTransform() { t.setIdentity(); }
   
     const shared_ptr<const CollisionGeometry> collisionGeometry() const {
       return cgeom;
     }
   
     const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
   
     const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); }
   
     CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); }
   
     void setCollisionGeometry(
         const shared_ptr<CollisionGeometry>& collision_geometry,
         bool compute_local_aabb = true) {
       if (collision_geometry.get() != cgeom.get()) {
         cgeom = collision_geometry;
         init(compute_local_aabb);
       }
     }
   
    protected:
     void init(bool compute_local_aabb = true) {
       if (cgeom) {
         if (compute_local_aabb) cgeom->computeLocalAABB();
         computeAABB();
       }
     }
   
     shared_ptr<CollisionGeometry> cgeom;
   
     Transform3s t;
   
     mutable AABB aabb;
   
     void* user_data;
   };
   
   }  // namespace coal
   
   #endif