.. _program_listing_file__tmp_ws_src_hpp-fcl_include_hpp_fcl_BV_AABB.h: Program Listing for File AABB.h =============================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/hpp-fcl/include/hpp/fcl/BV/AABB.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 HPP_FCL_AABB_H #define HPP_FCL_AABB_H #include namespace hpp { namespace fcl { struct CollisionRequest; class HPP_FCL_DLLAPI AABB { public: Vec3f min_; Vec3f max_; AABB(); AABB(const Vec3f& v) : min_(v), max_(v) {} AABB(const Vec3f& a, const Vec3f& b) : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta), max_(core.max_ + delta) {} AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} AABB(const AABB& other) = default; AABB& operator=(const AABB& other) = default; AABB& update(const Vec3f& a, const Vec3f& b) { min_ = a.cwiseMin(b); max_ = a.cwiseMax(b); return *this; } bool operator==(const AABB& other) const { return min_ == other.min_ && max_ == other.max_; } bool operator!=(const AABB& other) const { return !(*this == other); } inline bool contain(const Vec3f& p) const { if (p[0] < min_[0] || p[0] > max_[0]) return false; if (p[1] < min_[1] || p[1] > max_[1]) return false; if (p[2] < min_[2] || p[2] > max_[2]) return false; return true; } inline bool overlap(const AABB& other) const { if (min_[0] > other.max_[0]) return false; if (min_[1] > other.max_[1]) return false; if (min_[2] > other.max_[2]) return false; if (max_[0] < other.min_[0]) return false; if (max_[1] < other.min_[1]) return false; if (max_[2] < other.min_[2]) return false; return true; } bool overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const; FCL_REAL distance(const AABB& other) const; FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; inline AABB& operator+=(const Vec3f& p) { min_ = min_.cwiseMin(p); max_ = max_.cwiseMax(p); return *this; } inline AABB& operator+=(const AABB& other) { min_ = min_.cwiseMin(other.min_); max_ = max_.cwiseMax(other.max_); return *this; } inline AABB operator+(const AABB& other) const { AABB res(*this); return res += other; } inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); } inline Vec3f center() const { return (min_ + max_) * 0.5; } inline FCL_REAL width() const { return max_[0] - min_[0]; } inline FCL_REAL height() const { return max_[1] - min_[1]; } inline FCL_REAL depth() const { return max_[2] - min_[2]; } inline FCL_REAL volume() const { return width() * height() * depth(); } inline bool contain(const AABB& other) const { return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); } inline bool overlap(const AABB& other, AABB& overlap_part) const { if (!overlap(other)) { return false; } overlap_part.min_ = min_.cwiseMax(other.min_); overlap_part.max_ = max_.cwiseMin(other.max_); return true; } inline bool axisOverlap(const AABB& other, int axis_id) const { if (min_[axis_id] > other.max_[axis_id]) return false; if (max_[axis_id] < other.min_[axis_id]) return false; return true; } inline AABB& expand(const Vec3f& delta) { min_ -= delta; max_ += delta; return *this; } inline AABB& expand(const FCL_REAL delta) { min_.array() -= delta; max_.array() += delta; return *this; } inline AABB& expand(const AABB& core, FCL_REAL ratio) { min_ = min_ * ratio - core.min_; max_ = max_ * ratio - core.max_; return *this; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; static inline AABB translate(const AABB& aabb, const Vec3f& t) { AABB res(aabb); res.min_ += t; res.max_ += t; return res; } static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { AABB res(R * aabb.min_); Vec3f corner(aabb.min_); const Eigen::DenseIndex bit[3] = {1, 2, 4}; for (Eigen::DenseIndex ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. for (Eigen::DenseIndex i = 0; i < 3; ++i) { corner[i] = (ic & bit[i]) ? aabb.max_[i] : aabb.min_[i]; } res += R * corner; } return res; } HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2); HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); } // namespace fcl } // namespace hpp #endif