Program Listing for File collision_object.h
↰ Return to documentation for file (include/coal/collision_object.h)
/*
* 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