collision_object.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_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<CoalScalar>::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;

 CoalScalar aabb_radius;

 AABB aabb_local;

 void* user_data;

 CoalScalar cost_density;

 CoalScalar threshold_occupied;

 CoalScalar threshold_free;

 virtual Vec3s computeCOM() const { return Vec3s::Zero(); }

 virtual Matrix3s computeMomentofInertia() const {
 return Matrix3s::Constant(NAN);
 }

 virtual CoalScalar computeVolume() const { return 0; }

 virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
 Matrix3s C = computeMomentofInertia();
 Vec3s com = computeCOM();
 CoalScalar 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